├── .idea
├── .gitignore
├── inspectionProfiles
│ ├── Project_Default.xml
│ └── profiles_settings.xml
├── misc.xml
├── modules.xml
├── mpu6050_DMP_python.iml
└── vcs.xml
├── LICENSE
├── MPU6050RPI.py
├── MPU6050_cal.py
├── MPU6050_example.py
├── MPUConstants.py
├── Quaternion.py
├── README.md
├── SimplePID.py
├── csv.py
└── usmbus.py
/.idea/.gitignore:
--------------------------------------------------------------------------------
1 | # Default ignored files
2 | /shelf/
3 | /workspace.xml
4 | # Datasource local storage ignored files
5 | /../../../../../:\Workspace\proj\mpu6050_DMP_python\.idea/dataSources/
6 | /dataSources.local.xml
7 | # Editor-based HTTP Client requests
8 | /httpRequests/
9 |
--------------------------------------------------------------------------------
/.idea/inspectionProfiles/Project_Default.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/.idea/inspectionProfiles/profiles_settings.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/.idea/misc.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/.idea/mpu6050_DMP_python.iml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
--------------------------------------------------------------------------------
/.idea/vcs.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2021 Michał Banaś
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/MPU6050RPI.py:
--------------------------------------------------------------------------------
1 | __author__ = 'Geir Istad'
2 | """
3 | MPU6050 Python I2C Class
4 | Copyright (c) 2015 Geir Istad
5 |
6 | Permission is hereby granted, free of charge, to any person obtaining a copy
7 | of this software and associated documentation files (the "Software"), to deal
8 | in the Software without restriction, including without limitation the rights
9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 | copies of the Software, and to permit persons to whom the Software is
11 | furnished to do so, subject to the following conditions:
12 |
13 | The above copyright notice and this permission notice shall be included in all
14 | copies or substantial portions of the Software.
15 |
16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 | SOFTWARE.
23 |
24 |
25 | Code based on
26 | I2Cdev library collection - MPU6050 I2C device class
27 | by Jeff Rowberg
28 | ============================================
29 | I2Cdev device library code is placed under the MIT license
30 | Copyright (c) 2012 Jeff Rowberg
31 | Permission is hereby granted, free of charge, to any person obtaining a copy
32 | of this software and associated documentation files (the "Software"), to deal
33 | in the Software without restriction, including without limitation the rights
34 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
35 | copies of the Software, and to permit persons to whom the Software is
36 | furnished to do so, subject to the following conditions:
37 | The above copyright notice and this permission notice shall be included in
38 | all copies or substantial portions of the Software.
39 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
40 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
41 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
42 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
43 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
44 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
45 | THE SOFTWARE.
46 | ===============================================
47 | """
48 |
49 | import math
50 | # import ctypes
51 | import time
52 | import usmbus as smbus
53 | import csv
54 | from MPUConstants import MPUConstants as C
55 | from Quaternion import Quaternion as Q
56 | from Quaternion import XYZVector as V
57 |
58 | import struct
59 |
60 |
61 | class ValObject:
62 | value = None
63 |
64 | def __init__(self, val):
65 | self.value = val
66 |
67 |
68 | class CTYPES:
69 | def c_int16(self, data):
70 | v = (data).to_bytes(2, 'big')
71 | return ValObject(v)
72 |
73 | def c_int8(self, data):
74 | v = (data).to_bytes(1, 'big')
75 | return ValObject(v)
76 |
77 |
78 | ctypes = CTYPES()
79 | xrange = range
80 |
81 |
82 | def to_short(somebytes):
83 | return struct.unpack('>h', somebytes)[0]
84 |
85 |
86 | class MPU6050:
87 | __buffer = [0] * 14
88 | __debug = False
89 | __DMP_packet_size = 0
90 | __dev_id = 0
91 | __bus = None
92 |
93 | def __init__(self, a_bus=1, a_address=C.MPU6050_DEFAULT_ADDRESS,
94 | a_xAOff=None, a_yAOff=None, a_zAOff=None, a_xGOff=None,
95 | a_yGOff=None, a_zGOff=None, a_debug=False):
96 | self.__dev_id = a_address
97 | # Connect to num 1 SMBus
98 | self.__bus = smbus.SMBus(a_bus)
99 | # Set clock source to gyro
100 | self.set_clock_source(C.MPU6050_CLOCK_PLL_XGYRO)
101 | # Set accelerometer range
102 | self.set_full_scale_accel_range(C.MPU6050_ACCEL_FS_2)
103 | # Set gyro range
104 | self.set_full_scale_gyro_range(C.MPU6050_GYRO_FS_250)
105 | # Take the MPU out of time.sleep mode
106 | self.wake_up()
107 | # Set offsets
108 | if a_xAOff:
109 | self.set_x_accel_offset(a_xAOff)
110 | if a_yAOff:
111 | self.set_y_accel_offset(a_yAOff)
112 | if a_zAOff:
113 | self.set_z_accel_offset(a_zAOff)
114 | if a_xGOff:
115 | self.set_x_gyro_offset(a_xGOff)
116 | if a_yGOff:
117 | self.set_y_gyro_offset(a_yGOff)
118 | if a_zGOff:
119 | self.set_z_gyro_offset(a_zGOff)
120 | self.__debug = a_debug
121 |
122 | # Core bit and byte operations
123 | def read_bit(self, a_reg_add, a_bit_position):
124 | return self.read_bits(a_reg_add, a_bit_position, 1)
125 |
126 | def write_bit(self, a_reg_add, a_bit_num, a_bit):
127 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add)
128 | if a_bit:
129 | byte |= 1 << a_bit_num
130 | else:
131 | byte &= ~(1 << a_bit_num)
132 | self.__bus.write_byte_data(
133 | self.__dev_id, a_reg_add, ctypes.c_int8(byte).value)
134 |
135 | def read_bits(self, a_reg_add, a_bit_start, a_length):
136 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add)
137 | mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1)
138 | byte &= mask
139 | byte >>= a_bit_start - a_length + 1
140 | return byte
141 |
142 | def write_bits(self, a_reg_add, a_bit_start, a_length, a_data):
143 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add)
144 | mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1)
145 | # Get data in position and zero all non-important bits in data
146 | a_data <<= a_bit_start - a_length + 1
147 | a_data &= mask
148 | # Clear all important bits in read byte and combine with data
149 | byte &= ~mask
150 | byte = byte | a_data
151 | # Write the data to the I2C device
152 | self.__bus.write_byte_data(
153 | self.__dev_id, a_reg_add, ctypes.c_int8(byte).value)
154 |
155 | def read_memory_byte(self):
156 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_MEM_R_W)
157 |
158 | def read_bytes(self, a_data_list, a_address, a_length):
159 | if a_length > len(a_data_list):
160 | print('read_bytes, length of passed list too short')
161 | return a_data_list
162 | # Attempt to use the built in read bytes function in the adafruit lib
163 | # a_data_list = self.__bus.read_i2c_block_data(self.__dev_id, a_address,
164 | # a_length)
165 | # Attempt to bypass adafruit lib
166 | # a_data_list = self.__mpu.bus.read_i2c_block_data(0x68, a_address, a_length)
167 | # print('data' + str(a_data_list))
168 | for x in xrange(0, a_length):
169 | a_data_list[x] = self.__bus.read_byte_data(self.__dev_id,
170 | a_address + x)
171 | return a_data_list
172 |
173 | def write_memory_block(self, a_data_list, a_data_size, a_bank, a_address,
174 | a_verify):
175 | success = True
176 | self.set_memory_bank(a_bank)
177 | self.set_memory_start_address(a_address)
178 |
179 | # For each a_data_item we want to write it to the board to a certain
180 | # memory bank and address
181 | for i in range(0, a_data_size):
182 | # Write each data to memory
183 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_MEM_R_W,
184 | a_data_list[i])
185 |
186 | if a_verify:
187 | self.set_memory_bank(a_bank)
188 | self.set_memory_start_address(a_address)
189 | verify_data = self.__bus.read_byte_data(self.__dev_id,
190 | C.MPU6050_RA_MEM_R_W)
191 | if verify_data != a_data_list[i]:
192 | success = False
193 |
194 | # If we've filled the bank, change the memory bank
195 | if a_address == 255:
196 | a_address = 0
197 | a_bank += 1
198 | self.set_memory_bank(a_bank)
199 | else:
200 | a_address += 1
201 |
202 | # Either way update the memory address
203 | self.set_memory_start_address(a_address)
204 |
205 | return success
206 |
207 | def wake_up(self):
208 | self.write_bit(
209 | C.MPU6050_RA_PWR_MGMT_1, C.MPU6050_PWR1_SLEEP_BIT, 0)
210 |
211 | def set_clock_source(self, a_source):
212 | self.write_bits(C.MPU6050_RA_PWR_MGMT_1, C.MPU6050_PWR1_CLKSEL_BIT,
213 | C.MPU6050_PWR1_CLKSEL_LENGTH, a_source)
214 |
215 | def set_full_scale_gyro_range(self, a_data):
216 | self.write_bits(C.MPU6050_RA_GYRO_CONFIG,
217 | C.MPU6050_GCONFIG_FS_SEL_BIT,
218 | C.MPU6050_GCONFIG_FS_SEL_LENGTH, a_data)
219 |
220 | def set_full_scale_accel_range(self, a_data):
221 | self.write_bits(C.MPU6050_RA_ACCEL_CONFIG,
222 | C.MPU6050_ACONFIG_AFS_SEL_BIT,
223 | C.MPU6050_ACONFIG_AFS_SEL_LENGTH, a_data)
224 |
225 | def reset(self):
226 | self.write_bit(C.MPU6050_RA_PWR_MGMT_1,
227 | C.MPU6050_PWR1_DEVICE_RESET_BIT, 1)
228 |
229 | def set_sleep_enabled(self, a_enabled):
230 | set_bit = 0
231 | if a_enabled:
232 | set_bit = 1
233 | self.write_bit(C.MPU6050_RA_PWR_MGMT_1,
234 | C.MPU6050_PWR1_SLEEP_BIT, set_bit)
235 |
236 | def set_memory_bank(self, a_bank, a_prefetch_enabled=False,
237 | a_user_bank=False):
238 | a_bank &= 0x1F
239 | if a_user_bank:
240 | a_bank |= 0x20
241 | if a_prefetch_enabled:
242 | a_bank |= 0x20
243 | self.__bus.write_byte_data(
244 | self.__dev_id, C.MPU6050_RA_BANK_SEL, a_bank)
245 |
246 | def set_memory_start_address(self, a_address):
247 | self.__bus.write_byte_data(
248 | self.__dev_id, C.MPU6050_RA_MEM_START_ADDR, a_address)
249 |
250 | def get_x_gyro_offset_TC(self):
251 | return self.read_bits(C.MPU6050_RA_XG_OFFS_TC,
252 | C.MPU6050_TC_OFFSET_BIT,
253 | C.MPU6050_TC_OFFSET_LENGTH)
254 |
255 | def set_x_gyro_offset_TC(self, a_offset):
256 | self.write_bits(C.MPU6050_RA_XG_OFFS_TC,
257 | C.MPU6050_TC_OFFSET_BIT,
258 | C.MPU6050_TC_OFFSET_LENGTH, a_offset)
259 |
260 | def get_y_gyro_offset_TC(self):
261 | return self.read_bits(C.MPU6050_RA_YG_OFFS_TC,
262 | C.MPU6050_TC_OFFSET_BIT,
263 | C.MPU6050_TC_OFFSET_LENGTH)
264 |
265 | def set_y_gyro_offset_TC(self, a_offset):
266 | self.write_bits(C.MPU6050_RA_YG_OFFS_TC,
267 | C.MPU6050_TC_OFFSET_BIT,
268 | C.MPU6050_TC_OFFSET_LENGTH, a_offset)
269 |
270 | def get_z_gyro_offset_TC(self):
271 | return self.read_bits(C.MPU6050_RA_ZG_OFFS_TC,
272 | C.MPU6050_TC_OFFSET_BIT,
273 | C.MPU6050_TC_OFFSET_LENGTH)
274 |
275 | def set_z_gyro_offset_TC(self, a_offset):
276 | self.write_bits(C.MPU6050_RA_ZG_OFFS_TC,
277 | C.MPU6050_TC_OFFSET_BIT,
278 | C.MPU6050_TC_OFFSET_LENGTH, a_offset)
279 |
280 | def set_slave_address(self, a_num, a_address):
281 | self.__bus.write_byte_data(
282 | self.__dev_id, C.MPU6050_RA_I2C_SLV0_ADDR + a_num * 3, a_address)
283 |
284 | def set_I2C_master_mode_enabled(self, a_enabled):
285 | bit = 0
286 | if a_enabled:
287 | bit = 1
288 | self.write_bit(C.MPU6050_RA_USER_CTRL,
289 | C.MPU6050_USERCTRL_I2C_MST_EN_BIT, bit)
290 |
291 | def reset_I2C_master(self):
292 | self.write_bit(C.MPU6050_RA_USER_CTRL,
293 | C.MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1)
294 |
295 | def write_prog_memory_block(self, a_data_list, a_data_size, a_bank=0,
296 | a_address=0, a_verify=True):
297 | return self.write_memory_block(a_data_list, a_data_size, a_bank,
298 | a_address, a_verify)
299 |
300 | def write_DMP_configuration_set(self, a_data_list, a_data_size):
301 | index = 0
302 | while index < a_data_size:
303 | bank = a_data_list[index]
304 | offset = a_data_list[index + 1]
305 | length = a_data_list[index + 2]
306 | index += 3
307 | success = False
308 |
309 | # Normal case
310 | if length > 0:
311 | data_selection = list()
312 | for subindex in range(0, length):
313 | data_selection.append(a_data_list[index + subindex])
314 | success = self.write_memory_block(data_selection, length, bank,
315 | offset, True)
316 | index += length
317 | # Special undocumented case
318 | else:
319 | special = a_data_list[index]
320 | index += 1
321 | if special == 0x01:
322 | # TODO Figure out if write8 can return True/False
323 | success = self.__bus.write_byte_data(
324 | self.__dev_id, C.MPU6050_RA_INT_ENABLE, 0x32)
325 |
326 | if success == False:
327 | # TODO implement error messagemajigger
328 | return False
329 | pass
330 | return True
331 |
332 | def write_prog_dmp_configuration(self, a_data_list, a_data_size):
333 | return self.write_DMP_configuration_set(a_data_list, a_data_size)
334 |
335 | def set_int_enable(self, a_enabled):
336 | self.__bus.write_byte_data(
337 | self.__dev_id, C.MPU6050_RA_INT_ENABLE, a_enabled)
338 |
339 | def set_rate(self, a_rate):
340 | self.__bus.write_byte_data(
341 | self.__dev_id, C.MPU6050_RA_SMPLRT_DIV, a_rate)
342 |
343 | def set_external_frame_sync(self, a_sync):
344 | self.write_bits(C.MPU6050_RA_CONFIG,
345 | C.MPU6050_CFG_EXT_SYNC_SET_BIT,
346 | C.MPU6050_CFG_EXT_SYNC_SET_LENGTH, a_sync)
347 |
348 | def set_DLF_mode(self, a_mode):
349 | self.write_bits(C.MPU6050_RA_CONFIG, C.MPU6050_CFG_DLPF_CFG_BIT,
350 | C.MPU6050_CFG_DLPF_CFG_LENGTH, a_mode)
351 |
352 | def get_DMP_config_1(self):
353 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_DMP_CFG_1)
354 |
355 | def set_DMP_config_1(self, a_config):
356 | self.__bus.write_byte_data(
357 | self.__dev_id, C.MPU6050_RA_DMP_CFG_1, a_config)
358 |
359 | def get_DMP_config_2(self):
360 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_DMP_CFG_2)
361 |
362 | def set_DMP_config_2(self, a_config):
363 | self.__bus.write_byte_data(
364 | self.__dev_id, C.MPU6050_RA_DMP_CFG_2, a_config)
365 |
366 | def set_OTP_bank_valid(self, a_enabled):
367 | bit = 0
368 | if a_enabled:
369 | bit = 1
370 | self.write_bit(C.MPU6050_RA_XG_OFFS_TC,
371 | C.MPU6050_TC_OTP_BNK_VLD_BIT, bit)
372 |
373 | def get_OTP_bank_valid(self):
374 | return self.read_bit(C.MPU6050_RA_XG_OFFS_TC,
375 | C.MPU6050_TC_OTP_BNK_VLD_BIT)
376 |
377 | def set_motion_detection_threshold(self, a_threshold):
378 | self.__bus.write_byte_data(
379 | self.__dev_id, C.MPU6050_RA_MOT_THR, a_threshold)
380 |
381 | def set_zero_motion_detection_threshold(self, a_threshold):
382 | self.__bus.write_byte_data(
383 | self.__dev_id, C.MPU6050_RA_ZRMOT_THR, a_threshold)
384 |
385 | def set_motion_detection_duration(self, a_duration):
386 | self.__bus.write_byte_data(
387 | self.__dev_id, C.MPU6050_RA_MOT_DUR, a_duration)
388 |
389 | def set_zero_motion_detection_duration(self, a_duration):
390 | self.__bus.write_byte_data(
391 | self.__dev_id, C.MPU6050_RA_ZRMOT_DUR, a_duration)
392 |
393 | def set_FIFO_enabled(self, a_enabled):
394 | bit = 0
395 | if a_enabled:
396 | bit = 1
397 | self.write_bit(C.MPU6050_RA_USER_CTRL,
398 | C.MPU6050_USERCTRL_FIFO_EN_BIT, bit)
399 |
400 | def set_DMP_enabled(self, a_enabled):
401 | bit = 0
402 | if a_enabled:
403 | bit = 1
404 | self.write_bit(C.MPU6050_RA_USER_CTRL,
405 | C.MPU6050_USERCTRL_DMP_EN_BIT, bit)
406 |
407 | def reset_DMP(self):
408 | self.write_bit(C.MPU6050_RA_USER_CTRL,
409 | C.MPU6050_USERCTRL_DMP_RESET_BIT, True)
410 |
411 | def dmp_initialize(self):
412 | # Reset the MPU
413 | self.reset()
414 | # time.Sleep a bit while resetting
415 | time.sleep(50 / 1000)
416 | # Disable time.sleep mode
417 | self.set_sleep_enabled(0)
418 |
419 | # get MPU hardware revision
420 | if self.__debug:
421 | print('Selecting user bank 16')
422 | self.set_memory_bank(0x10, True, True)
423 |
424 | if self.__debug:
425 | print('Selecting memory byte 6')
426 | self.set_memory_start_address(0x6)
427 |
428 | if self.__debug:
429 | print('Checking hardware revision')
430 | HW_revision = self.read_memory_byte()
431 | if self.__debug:
432 | print('Revision @ user[16][6] = ' + hex(HW_revision))
433 |
434 | if self.__debug:
435 | print('Resetting memory bank selection to 0')
436 | self.set_memory_bank(0)
437 |
438 | # check OTP bank valid
439 | # TODO Find out what OTP is
440 | OTP_valid = self.get_OTP_bank_valid()
441 | if self.__debug:
442 | if OTP_valid:
443 | print('OTP bank is valid')
444 | else:
445 | print('OTP bank is invalid')
446 |
447 | # get X/Y/Z gyro offsets
448 | if self.__debug:
449 | print('Reading gyro offet TC values')
450 | x_g_offset_TC = self.get_x_gyro_offset_TC()
451 | y_g_offset_TC = self.get_y_gyro_offset_TC()
452 | z_g_offset_TC = self.get_z_gyro_offset_TC()
453 | if self.__debug:
454 | print("X gyro offset = ", repr(x_g_offset_TC))
455 | print("Y gyro offset = ", repr(y_g_offset_TC))
456 | print("Z gyro offset = ", repr(z_g_offset_TC))
457 |
458 | # setup weird slave stuff (?)
459 | if self.__debug:
460 | print('Setting slave 0 address to 0x7F')
461 | self.set_slave_address(0, 0x7F)
462 | if self.__debug:
463 | print('Disabling I2C Master mode')
464 | self.set_I2C_master_mode_enabled(False)
465 | if self.__debug:
466 | print('Setting slave 0 address to 0x68 (self)')
467 | self.set_slave_address(0, 0x68)
468 | if self.__debug:
469 | print('Resetting I2C Master control')
470 | self.reset_I2C_master()
471 | # Wait a bit for the device to register the changes
472 | time.sleep(20 / 1000)
473 |
474 | # load DMP code into memory banks
475 | if self.__debug:
476 | print('Writing DMP code to MPU memory banks ' +
477 | repr(C.MPU6050_DMP_CODE_SIZE) + ' bytes')
478 | if self.write_prog_memory_block(C.dmpMemory, C.MPU6050_DMP_CODE_SIZE):
479 | # TODO Check if we've actually verified this
480 | if self.__debug:
481 | print('Success! DMP code written and verified')
482 |
483 | # Write DMP configuration
484 | if self.__debug:
485 | print('Writing DMP configuration to MPU memory banks ' +
486 | repr(C.MPU6050_DMP_CONFIG_SIZE) + ' bytes in config')
487 | if self.write_prog_dmp_configuration(C.dmpConfig,
488 | C.MPU6050_DMP_CONFIG_SIZE):
489 | if self.__debug:
490 | print('Success! DMP configuration written and verified.')
491 | print('Setting clock source to Z gyro')
492 | self.set_clock_source(C.MPU6050_CLOCK_PLL_ZGYRO)
493 |
494 | if self.__debug:
495 | print('Setting DMP and FIFO_OFLOW interrupts enabled')
496 | self.set_int_enable(0x12)
497 |
498 | if self.__debug:
499 | print('Setting sample rate to 200Hz')
500 | self.set_rate(4)
501 |
502 | if self.__debug:
503 | print('Setting external frame sync to TEMP_OUT_L[0]')
504 | self.set_external_frame_sync(C.MPU6050_EXT_SYNC_TEMP_OUT_L)
505 |
506 | if self.__debug:
507 | print('Setting DLPF bandwidth to 42Hz')
508 | self.set_DLF_mode(C.MPU6050_DLPF_BW_42)
509 |
510 | if self.__debug:
511 | print('Setting gyro sensitivity to +/- 2000 deg/sec')
512 | self.set_full_scale_gyro_range(C.MPU6050_GYRO_FS_2000)
513 |
514 | if self.__debug:
515 | print('Setting DMP configuration bytes (function unknown)')
516 | self.set_DMP_config_1(0x03)
517 | self.set_DMP_config_2(0x00)
518 |
519 | if self.__debug:
520 | print('Clearing OTP Bank flag')
521 | self.set_OTP_bank_valid(False)
522 |
523 | if self.__debug:
524 | print('Setting X/Y/Z gyro offset TCs to previous values')
525 | self.set_x_gyro_offset_TC(x_g_offset_TC)
526 | self.set_y_gyro_offset_TC(y_g_offset_TC)
527 | self.set_z_gyro_offset_TC(z_g_offset_TC)
528 |
529 | # Uncomment this to zero offsets when dmp_initialize is called
530 | # if self.__debug:
531 | # print('Setting X/Y/Z gyro user offsets to zero')
532 | # self.set_x_gyro_offset(0)
533 | # self.set_y_gyro_offset(0)
534 | # self.set_z_gyro_offset(0)
535 |
536 | if self.__debug:
537 | print('Writing final memory update 1/7 (function unknown)')
538 | pos = 0
539 | j = 0
540 | dmp_update = [0] * 16
541 | while (j < 4) or (j < dmp_update[2] + 3):
542 | dmp_update[j] = C.dmpUpdates[pos]
543 | pos += 1
544 | j += 1
545 | # Write as block from pos 3
546 | self.write_memory_block(dmp_update[3:], dmp_update[2],
547 | dmp_update[0], dmp_update[1], True)
548 |
549 | if self.__debug:
550 | print('Writing final memory update 2/7 (function unknown)')
551 | j = 0
552 | while (j < 4) or (j < dmp_update[2] + 3):
553 | dmp_update[j] = C.dmpUpdates[pos]
554 | pos += 1
555 | j += 1
556 | # Write as block from pos 3
557 | self.write_memory_block(dmp_update[3:], dmp_update[2],
558 | dmp_update[0], dmp_update[1], True)
559 |
560 | if self.__debug:
561 | print('Resetting FIFO')
562 | self.reset_FIFO()
563 |
564 | if self.__debug:
565 | print('Reading FIFO count')
566 | FIFO_count = self.get_FIFO_count()
567 |
568 | if self.__debug:
569 | print('FIFO count: ' + repr(FIFO_count))
570 |
571 | if self.__debug:
572 | print('Getting FIFO buffer')
573 | FIFO_buffer = [0] * 128
574 | FIFO_buffer = self.get_FIFO_bytes(FIFO_count)
575 |
576 | if self.__debug:
577 | print('Setting motion detection threshold to 2')
578 | self.set_motion_detection_threshold(2)
579 |
580 | if self.__debug:
581 | print('Setting zero-motion detection threshold to 156')
582 | self.set_zero_motion_detection_threshold(156)
583 |
584 | if self.__debug:
585 | print('Setting motion detection duration to 80')
586 | self.set_motion_detection_duration(80)
587 |
588 | if self.__debug:
589 | print('Setting zero-motion detection duration to 0')
590 | self.set_zero_motion_detection_duration(0)
591 |
592 | if self.__debug:
593 | print('Resetting FIFO')
594 | self.reset_FIFO()
595 |
596 | if self.__debug:
597 | print('Enabling FIFO')
598 | self.set_FIFO_enabled(True)
599 |
600 | if self.__debug:
601 | print('Enabling DMP')
602 | self.set_DMP_enabled(True)
603 |
604 | if self.__debug:
605 | print('Resetting DMP')
606 | self.reset_DMP()
607 |
608 | if self.__debug:
609 | print('Writing final memory update 3/7 (function unknown)')
610 | j = 0
611 | while (j < 4) or (j < dmp_update[2] + 3):
612 | dmp_update[j] = C.dmpUpdates[pos]
613 | pos += 1
614 | j += 1
615 | # Write as block from pos 3
616 | self.write_memory_block(dmp_update[3:], dmp_update[2],
617 | dmp_update[0], dmp_update[1], True)
618 |
619 | if self.__debug:
620 | print('Writing final memory update 4/7 (function unknown)')
621 | j = 0
622 | while (j < 4) or (j < dmp_update[2] + 3):
623 | dmp_update[j] = C.dmpUpdates[pos]
624 | pos += 1
625 | j += 1
626 | # Write as block from pos 3
627 | self.write_memory_block(dmp_update[3:], dmp_update[2],
628 | dmp_update[0], dmp_update[1], True)
629 |
630 | if self.__debug:
631 | print('Writing final memory update 5/7 (function unknown)')
632 | j = 0
633 | while (j < 4) or (j < dmp_update[2] + 3):
634 | dmp_update[j] = C.dmpUpdates[pos]
635 | pos += 1
636 | j += 1
637 | # Write as block from pos 3
638 | self.write_memory_block(dmp_update[3:], dmp_update[2],
639 | dmp_update[0], dmp_update[1], True)
640 |
641 | if self.__debug:
642 | print('Waiting for FIFO count > 2')
643 | FIFO_count = self.get_FIFO_count()
644 | while FIFO_count < 3:
645 | FIFO_count = self.get_FIFO_count()
646 |
647 | if self.__debug:
648 | print('Current FIFO count = ' + repr(FIFO_count))
649 | print('Reading FIFO data')
650 | FIFO_buffer = self.get_FIFO_bytes(FIFO_count)
651 |
652 | if self.__debug:
653 | print('Reading interrupt status')
654 | MPU_int_status = self.get_int_status()
655 |
656 | if self.__debug:
657 | print('Current interrupt status = ' + hex(MPU_int_status))
658 | print('Writing final memory update 6/7 (function unknown)')
659 | j = 0
660 | while (j < 4) or (j < dmp_update[2] + 3):
661 | dmp_update[j] = C.dmpUpdates[pos]
662 | pos += 1
663 | j += 1
664 | # Write as block from pos 3
665 | self.write_memory_block(dmp_update[3:], dmp_update[2],
666 | dmp_update[0], dmp_update[1], True)
667 |
668 | if self.__debug:
669 | print('Waiting for FIFO count > 2')
670 | FIFO_count = self.get_FIFO_count()
671 | while FIFO_count < 3:
672 | FIFO_count = self.get_FIFO_count()
673 |
674 | if self.__debug:
675 | print('Current FIFO count = ' + repr(FIFO_count))
676 | print('Reading FIFO data')
677 | FIFO_buffer = self.get_FIFO_bytes(FIFO_count)
678 |
679 | if self.__debug:
680 | print('Reading interrupt status')
681 | MPU_int_status = self.get_int_status()
682 |
683 | if self.__debug:
684 | print('Current interrupt status = ' + hex(MPU_int_status))
685 | print('Writing final memory update 7/7 (function unknown)')
686 | j = 0
687 | while (j < 4) or (j < dmp_update[2] + 3):
688 | dmp_update[j] = C.dmpUpdates[pos]
689 | pos += 1
690 | j += 1
691 | # Write as block from pos 3
692 | self.write_memory_block(dmp_update[3:], dmp_update[2],
693 | dmp_update[0], dmp_update[1], True)
694 |
695 | if self.__debug:
696 | print('DMP is good to go! Finally.')
697 | print('Disabling DMP (you turn it on later)')
698 | self.set_DMP_enabled(False)
699 |
700 | if self.__debug:
701 | print('Setting up internal 42 byte DMP packet buffer')
702 | self.__DMP_packet_size = 42
703 |
704 | if self.__debug:
705 | print(
706 | 'Resetting FIFO and clearing INT status one last time')
707 | self.reset_FIFO()
708 | self.get_int_status()
709 |
710 | else:
711 | if self.__debug:
712 | print('Configuration block loading failed')
713 | return 2
714 |
715 | else:
716 | if self.__debug:
717 | print('Main binary block loading failed')
718 | return 1
719 |
720 | if self.__debug:
721 | print('DMP initialization was successful')
722 | return 0
723 |
724 | # Acceleration and gyro offset setters and getters
725 | def set_x_accel_offset(self, a_offset):
726 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XA_OFFS_H,
727 | ctypes.c_int8(a_offset >> 8).value)
728 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XA_OFFS_L_TC,
729 | ctypes.c_int8(a_offset).value)
730 |
731 | def set_y_accel_offset(self, a_offset):
732 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YA_OFFS_H,
733 | ctypes.c_int8(a_offset >> 8).value)
734 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YA_OFFS_L_TC,
735 | ctypes.c_int8(a_offset).value)
736 |
737 | def set_z_accel_offset(self, a_offset):
738 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZA_OFFS_H,
739 | ctypes.c_int8(a_offset >> 8).value)
740 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZA_OFFS_L_TC,
741 | ctypes.c_int8(a_offset).value)
742 |
743 | def set_x_gyro_offset(self, a_offset):
744 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XG_OFFS_USRH,
745 | ctypes.c_int8(a_offset >> 8).value)
746 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XG_OFFS_USRL,
747 | ctypes.c_int8(a_offset).value)
748 |
749 | def set_y_gyro_offset(self, a_offset):
750 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YG_OFFS_USRH,
751 | ctypes.c_int8(a_offset >> 8).value)
752 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YG_OFFS_USRL,
753 | ctypes.c_int8(a_offset).value)
754 |
755 | def set_z_gyro_offset(self, a_offset):
756 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZG_OFFS_USRH,
757 | ctypes.c_int8(a_offset >> 8).value)
758 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZG_OFFS_USRL,
759 | ctypes.c_int8(a_offset).value)
760 |
761 | # Main interfacing functions to get raw data from MPU
762 | def get_acceleration(self):
763 | raw_data = self.__bus.read_i2c_block_data(self.__dev_id,
764 | C.MPU6050_RA_ACCEL_XOUT_H, 6)
765 | accel = [0] * 3
766 | accel[0] = ctypes.c_int16(raw_data[0] << 8 | raw_data[1]).value
767 | accel[1] = ctypes.c_int16(raw_data[2] << 8 | raw_data[3]).value
768 | accel[2] = ctypes.c_int16(raw_data[4] << 8 | raw_data[5]).value
769 | return accel
770 |
771 | def get_rotation(self):
772 | raw_data = self.__bus.read_i2c_block_data(self.__dev_id,
773 | C.MPU6050_RA_GYRO_XOUT_H, 6)
774 | gyro = [0] * 3
775 | gyro[0] = ctypes.c_int16(raw_data[0] << 8 | raw_data[1]).value
776 | gyro[1] = ctypes.c_int16(raw_data[2] << 8 | raw_data[3]).value
777 | gyro[2] = ctypes.c_int16(raw_data[4] << 8 | raw_data[5]).value
778 | return gyro
779 |
780 | # Interfacing functions to get data from FIFO buffer
781 | def DMP_get_FIFO_packet_size(self):
782 | return self.__DMP_packet_size
783 |
784 | def reset_FIFO(self):
785 | self.write_bit(C.MPU6050_RA_USER_CTRL,
786 | C.MPU6050_USERCTRL_FIFO_RESET_BIT, True)
787 |
788 | def get_FIFO_count(self):
789 | data = [0] * 2
790 | data = self.read_bytes(data, C.MPU6050_RA_FIFO_COUNTH, 2)
791 | return (data[0] << 8) | data[1]
792 |
793 | def get_FIFO_bytes(self, a_FIFO_count):
794 | return_list = list()
795 | for index in range(0, a_FIFO_count):
796 | return_list.append(
797 | self.__bus.read_byte_data(self.__dev_id,
798 | C.MPU6050_RA_FIFO_R_W))
799 | return return_list
800 |
801 | def get_int_status(self):
802 | return self.__bus.read_byte_data(self.__dev_id,
803 | C.MPU6050_RA_INT_STATUS)
804 |
805 | # Data retrieval from received FIFO buffer
806 | def DMP_get_quaternion_int16(self, a_FIFO_buffer):
807 | w = ctypes.c_int16((a_FIFO_buffer[0] << 8) | a_FIFO_buffer[1]).value
808 | x = ctypes.c_int16((a_FIFO_buffer[4] << 8) | a_FIFO_buffer[5]).value
809 | y = ctypes.c_int16((a_FIFO_buffer[8] << 8) | a_FIFO_buffer[9]).value
810 | z = ctypes.c_int16((a_FIFO_buffer[12] << 8) | a_FIFO_buffer[13]).value
811 | return Q(w, x, y, z)
812 |
813 | def DMP_get_quaternion(self, a_FIFO_buffer):
814 | quat = self.DMP_get_quaternion_int16(a_FIFO_buffer)
815 | w = quat.w / 16384.0
816 | x = quat.x / 16384.0
817 | y = quat.y / 16384.0
818 | z = quat.z / 16384.0
819 | return Q(w, x, y, z)
820 |
821 | def DMP_get_acceleration_int16(self, a_FIFO_buffer):
822 | x = ctypes.c_int16(a_FIFO_buffer[28] << 8 | a_FIFO_buffer[29]).value
823 | y = ctypes.c_int16(a_FIFO_buffer[32] << 8 | a_FIFO_buffer[33]).value
824 | z = ctypes.c_int16(a_FIFO_buffer[36] << 8 | a_FIFO_buffer[37]).value
825 | return V(x, y, z)
826 |
827 | def DMP_get_gravity(self, a_quat):
828 | # it seems to be shortdata
829 |
830 | # original x = 2.0 * (a_quat.x * a_quat.z - a_quat.w * a_quat.y)
831 | # y = 2.0 * (a_quat.w * a_quat.x + a_quat.y * a_quat.z)
832 | # z = 1.0 * (a_quat.w * a_quat.w - a_quat.x * a_quat.x -
833 | # a_quat.y * a_quat.y + a_quat.z * a_quat.z)
834 | ax = to_short(a_quat.x)
835 | ay = to_short(a_quat.y)
836 | az = to_short(a_quat.z)
837 | aw = to_short(a_quat.w)
838 | x = 2.0 * (ax * az - aw * ay)
839 | y = 2.0 * (aw * ax + ay * az)
840 | z = 1.0 * (aw * aw - ax * ax -
841 | ay * ay + az * az)
842 |
843 | return V(x, y, z)
844 |
845 | def DMP_get_linear_accel_int16(self, a_v_raw, a_grav):
846 | x = ctypes.c_int16(a_v_raw.x - (a_grav.x * 8192)).value
847 | y = ctypes.c_int16(a_v_raw.y - (a_grav.y * 8192)).value
848 | y = ctypes.c_int16(a_v_raw.y - (a_grav.y * 8192)).value
849 | return V(x, y, z)
850 |
851 | def DMP_get_euler(self, a_quat):
852 | psi = math.atan2(2 * a_quat.x * a_quat.y - 2 * a_quat.w * a_quat.z,
853 | 2 * a_quat.w * a_quat.w + 2 * a_quat.x * a_quat.x - 1)
854 | theta = -asin(2 * a_quat.x * a_quat.z + 2 * a_quat.w * a_quat.y)
855 | phi = math.atan2(2 * a_quat.y * a_quat.z - 2 * a_quat.w * a_quat.x,
856 | 2 * a_quat.w * a_quat.w + 2 * a_quat.z * a_quat.z - 1)
857 | return V(psi, theta, phi)
858 |
859 | def DMP_get_roll_pitch_yaw(self, a_quat, a_grav_vect):
860 |
861 | # roll: (tilt left/right, about X axis)
862 | roll = math.atan(a_grav_vect.y /
863 | math.sqrt(a_grav_vect.x * a_grav_vect.x +
864 | a_grav_vect.z * a_grav_vect.z))
865 | # pitch: (nose up/down, about Y axis)
866 | pitch = math.atan(a_grav_vect.x /
867 | math.sqrt(a_grav_vect.y * a_grav_vect.y +
868 | a_grav_vect.z * a_grav_vect.z))
869 | # yaw: (about Z axis)
870 | # yaw = math.atan2(2*a_quat.x*a_quat.y - 2*a_quat.w*a_quat.z,
871 | # 2*a_quat.w*a_quat.w + 2*a_quat.x*a_quat.x - 1)
872 | yaw = math.atan2(2 * to_short(a_quat.x) * to_short(a_quat.y) - 2 * to_short(a_quat.w) * to_short(a_quat.z),
873 | 2 * to_short(a_quat.w) * to_short(a_quat.w) + 2 * to_short(a_quat.x) * to_short(a_quat.x) - 1)
874 |
875 | return V(roll, pitch, yaw)
876 |
877 | def DMP_get_euler_roll_pitch_yaw(self, a_quat, a_grav_vect):
878 | # rad_ypr = self.DMP_get_roll_pitch_yaw(a_quat, a_grav_vect)
879 | rad_ypr = self.DMP_get_roll_pitch_yaw(a_quat, a_grav_vect)
880 | roll = rad_ypr.x * (180.0 / math.pi)
881 | pitch = rad_ypr.y * (180.0 / math.pi)
882 | yaw = rad_ypr.z * (180.0 / math.pi)
883 | return V(roll, pitch, yaw)
884 |
885 | def DMP_get_linear_accel(self, a_vector_raw, a_vect_grav):
886 | x = a_vector_raw.x - a_vect_grav.x * 8192
887 | y = a_vector_raw.y - a_vect_grav.y * 8192
888 | z = a_vector_raw.z - a_vect_grav.z * 8192
889 | return V(x, y, z)
890 |
891 |
892 | class MPU6050IRQHandler:
893 | __mpu = MPU6050
894 | __FIFO_buffer = list()
895 | __count = 0
896 | __packet_size = None
897 | __detected_error = False
898 | __logging = False
899 | __log_file = None
900 | __csv_writer = None
901 | __start_time = None
902 | __debug = None
903 |
904 | # def __init__(self, a_i2c_bus, a_device_address, a_x_accel_offset,
905 | # a_y_accel_offset, a_z_accel_offset, a_x_gyro_offset,
906 | # a_y_gyro_offset, a_z_gyro_offset, a_enable_debug_output):
907 | # self.__mpu = MPU6050(a_i2c_bus, a_device_address, a_x_accel_offset,
908 | # a_y_accel_offset, a_z_accel_offset,
909 | # a_x_gyro_offset, a_y_gyro_offset, a_z_gyro_offset,
910 | # a_enable_debug_output)
911 | def __init__(self, a_mpu, a_logging=False, a_log_file='log.csv',
912 | a_debug=False):
913 | self.__mpu = a_mpu
914 | self.__FIFO_buffer = [0] * 64
915 | self.__mpu.dmp_initialize()
916 | self.__mpu.set_DMP_enabled(True)
917 | self.__packet_size = self.__mpu.DMP_get_FIFO_packet_size()
918 | mpu_int_status = self.__mpu.get_int_status()
919 | if a_logging:
920 | self.__start_time = time.clock()
921 | self.__logging = True
922 | self.__log_file = open(a_log_file, 'ab')
923 | self.__csv_writer = csv.writer(self.__log_file, delimiter=',',
924 | quotechar='|',
925 | quoting=csv.QUOTE_MINIMAL)
926 | self.__debug = a_debug
927 |
928 | def action(self, channel):
929 | if self.__detected_error:
930 | # Clear FIFO and reset MPU
931 | mpu_int_status = self.__mpu.get_int_status()
932 | self.__mpu.reset_FIFO()
933 | self.__detected_error = False
934 | return
935 |
936 | try:
937 | FIFO_count = self.__mpu.get_FIFO_count()
938 | mpu_int_status = self.__mpu.get_int_status()
939 | except:
940 | self.__detected_error = True
941 | return
942 |
943 | # If overflow is detected by status or fifo count we want to reset
944 | if (FIFO_count == 1024) or (mpu_int_status & 0x10):
945 | try:
946 | self.__mpu.reset_FIFO()
947 | except:
948 | self.__detected_error = True
949 | return
950 |
951 | elif (mpu_int_status & 0x02):
952 | # Wait until packet_size number of bytes are ready for reading,
953 | # default is 42 bytes
954 | while FIFO_count < self.__packet_size:
955 | try:
956 | FIFO_count = self.__mpu.get_FIFO_count()
957 | except:
958 | self.__detected_error = True
959 | return
960 |
961 | while FIFO_count > self.__packet_size:
962 |
963 | try:
964 | self.__FIFO_buffer = \
965 | self.__mpu.get_FIFO_bytes(self.__packet_size)
966 | except:
967 | self.__detected_error = True
968 | return
969 | accel = \
970 | self.__mpu.DMP_get_acceleration_int16(self.__FIFO_buffer)
971 | quat = self.__mpu.DMP_get_quaternion_int16(self.__FIFO_buffer)
972 | grav = self.__mpu.DMP_get_gravity(quat)
973 | roll_pitch_yaw = self.__mpu.DMP_get_euler_roll_pitch_yaw(quat,
974 | grav)
975 | if self.__logging:
976 | delta_time = time.clock() - self.__start_time
977 | data_concat = ['%.4f' % delta_time] + \
978 | [accel.x, accel.y, accel.z] + \
979 | ['%.3f' % roll_pitch_yaw.x,
980 | '%.3f' % roll_pitch_yaw.y,
981 | '%.3f' % roll_pitch_yaw.z]
982 | self.__csv_writer.writerow(data_concat)
983 |
984 | if (self.__debug) and (self.__count % 100 == 0):
985 | print('roll: ' + str(roll_pitch_yaw.x))
986 | print('pitch: ' + str(roll_pitch_yaw.y))
987 | print('yaw: ' + str(roll_pitch_yaw.z))
988 | self.__count += 1
989 | FIFO_count -= self.__packet_size
990 |
--------------------------------------------------------------------------------
/MPU6050_cal.py:
--------------------------------------------------------------------------------
1 | from MPU6050RPI import MPU6050
2 | from SimplePID import SimplePID
3 | import struct
4 |
5 |
6 | def to_short(somebytes):
7 | return struct.unpack('>h', somebytes)[0]
8 |
9 |
10 | def avg_from_array(a_array):
11 | sum = 0.0
12 |
13 | for index in range(0, len(a_array)):
14 | if isinstance(a_array[index], (bytes, bytearray)):
15 | a_array[index] = to_short(a_array[index])
16 | sum += a_array[index]
17 |
18 | return sum / len(a_array)
19 |
20 |
21 | i2c_bus = 1
22 | device_address = 0x68
23 | # The offsets are different for each device and should be changed
24 | # accordingly using a calibration procedure
25 | x_accel_offset = 0
26 | y_accel_offset = 0
27 | z_accel_offset = 0
28 | x_gyro_offset = 0
29 | y_gyro_offset = 0
30 | z_gyro_offset = 0
31 | enable_debug_output = True
32 |
33 | mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset,
34 | z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,
35 | enable_debug_output)
36 |
37 | kp = 0.03125
38 | ki = 0.25
39 | kd = 0
40 |
41 | pidax = SimplePID(0, -150, 15000, kp, ki, kd, 100, True)
42 | piday = SimplePID(0, -150, 15000, kp, ki, kd, 100, True)
43 | pidaz = SimplePID(0, -150, 15000, kp, ki, kd, 100, True)
44 | pidgx = SimplePID(0, -150, 15000, kp, ki, kd, 100, True)
45 | pidgy = SimplePID(0, -150, 15000, kp, ki, kd, 100, True)
46 | pidgz = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True)
47 |
48 | accel_reading = mpu.get_acceleration()
49 |
50 | x_accel_reading = accel_reading[0]
51 | y_accel_reading = accel_reading[1]
52 | z_accel_reading = accel_reading[2]
53 |
54 | x_accel_avg = [0] * 100
55 | y_accel_avg = [0] * 100
56 | z_accel_avg = [0] * 100
57 |
58 | x_accel_offset_avg = [0] * 100
59 | y_accel_offset_avg = [0] * 100
60 | z_accel_offset_avg = [0] * 100
61 |
62 | axindex = 0
63 | ayindex = 0
64 | azindex = 0
65 |
66 | gyro_reading = mpu.get_rotation()
67 |
68 | x_gyro_reading = gyro_reading[0]
69 | y_gyro_reading = gyro_reading[1]
70 | z_gyro_reading = gyro_reading[2]
71 |
72 | x_gyro_avg = [0] * 100
73 | y_gyro_avg = [0] * 100
74 | z_gyro_avg = [0] * 100
75 |
76 | x_gyro_offset_avg = [0] * 100
77 | y_gyro_offset_avg = [0] * 100
78 | z_gyro_offset_avg = [0] * 100
79 |
80 | gxindex = 0
81 | gyindex = 0
82 | gzindex = 0
83 |
84 | try:
85 | while True:
86 | accel_reading = mpu.get_acceleration()
87 | x_accel_reading = accel_reading[0]
88 | y_accel_reading = accel_reading[1]
89 | z_accel_reading = accel_reading[2]
90 |
91 | gyro_reading = mpu.get_rotation()
92 | x_gyro_reading = gyro_reading[0]
93 | y_gyro_reading = gyro_reading[1]
94 | z_gyro_reading = gyro_reading[2]
95 |
96 | if pidax.check_time():
97 | x_accel_offset = pidax.get_output_value(to_short(x_accel_reading))
98 |
99 | mpu.set_x_accel_offset(int(x_accel_offset))
100 |
101 | x_accel_avg[axindex] = x_accel_reading
102 | x_accel_offset_avg[axindex] = x_accel_offset
103 |
104 | axindex += 1
105 | if axindex == len(x_accel_avg):
106 | axindex = 0
107 | print('x_avg_read: ' +
108 | str(avg_from_array(x_accel_avg)) +
109 | ' x_avg_offset: ' +
110 | str(avg_from_array(x_accel_offset_avg)))
111 | print('y_avg_read: ' +
112 | str(avg_from_array(y_accel_avg)) +
113 | ' y_avg_offset: ' +
114 | str(avg_from_array(y_accel_offset_avg)))
115 | print('z_avg_read: ' +
116 | str(avg_from_array(z_accel_avg)) +
117 | ' z_avg_offset: ' +
118 | str(avg_from_array(z_accel_offset_avg)))
119 |
120 | if piday.check_time():
121 | y_accel_offset = piday.get_output_value(to_short(y_accel_reading))
122 |
123 | mpu.set_y_accel_offset(int(y_accel_offset))
124 |
125 | y_accel_avg[ayindex] = y_accel_reading
126 | y_accel_offset_avg[ayindex] = y_accel_offset
127 |
128 | ayindex += 1
129 | if ayindex == len(y_accel_avg):
130 | ayindex = 0
131 |
132 | if pidaz.check_time():
133 | z_accel_offset = pidaz.get_output_value(to_short(z_accel_reading))
134 |
135 | mpu.set_z_accel_offset(int(z_accel_offset))
136 |
137 | z_accel_avg[azindex] = z_accel_reading
138 | z_accel_offset_avg[azindex] = z_accel_offset
139 |
140 | azindex += 1
141 | if azindex == len(z_accel_avg):
142 | azindex = 0
143 |
144 | # Gyro calibration
145 | if pidgx.check_time():
146 | x_gyro_offset = pidgx.get_output_value(to_short(x_gyro_reading))
147 |
148 | mpu.set_x_gyro_offset(int(x_gyro_offset))
149 |
150 | x_gyro_avg[gxindex] = x_gyro_reading
151 | x_gyro_offset_avg[gxindex] = x_gyro_offset
152 |
153 | gxindex += 1
154 | if gxindex == len(x_gyro_avg):
155 | gxindex = 0
156 | print('x_avg_read_gyro: ' +
157 | str(avg_from_array(x_gyro_avg)) +
158 | ' x_avg_offset: ' +
159 | str(avg_from_array(x_gyro_offset_avg)))
160 | print('y_avg_read_gyro: ' +
161 | str(avg_from_array(y_gyro_avg)) +
162 | ' y_avg_offset: ' +
163 | str(avg_from_array(y_gyro_offset_avg)))
164 | print('z_avg_read_gyro: ' +
165 | str(avg_from_array(z_gyro_avg)) +
166 | ' z_avg_offset: ' +
167 | str(avg_from_array(z_gyro_offset_avg)))
168 |
169 | if pidgy.check_time():
170 | y_gyro_offset = pidgy.get_output_value(to_short(y_gyro_reading))
171 |
172 | mpu.set_y_gyro_offset(int(y_gyro_offset))
173 |
174 | y_gyro_avg[gyindex] = y_gyro_reading
175 | y_gyro_offset_avg[gyindex] = y_gyro_offset
176 |
177 | gyindex += 1
178 | if gyindex == len(y_gyro_avg):
179 | gyindex = 0
180 |
181 | if pidgz.check_time():
182 | z_gyro_offset = pidgz.get_output_value(to_short(z_gyro_reading))
183 |
184 | mpu.set_z_gyro_offset(int(z_gyro_offset))
185 |
186 | z_gyro_avg[gzindex] = z_gyro_reading
187 | z_gyro_offset_avg[gzindex] = z_gyro_offset
188 |
189 | gzindex += 1
190 | if gzindex == len(z_gyro_avg):
191 | gzindex = 0
192 |
193 | except KeyboardInterrupt:
194 | pass
195 |
196 |
--------------------------------------------------------------------------------
/MPU6050_example.py:
--------------------------------------------------------------------------------
1 | __author__ = 'Geir Istad'
2 | """
3 | MPU6050 Python I2C Class - MPU6050 example usage
4 | Copyright (c) 2015 Geir Istad
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | The above copyright notice and this permission notice shall be included in all
12 | copies or substantial portions of the Software.
13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19 | SOFTWARE.
20 | """
21 |
22 | from MPU6050RPI import MPU6050
23 |
24 | i2c_bus = 1
25 | device_address = 0x68
26 | # The offsets are different for each device and should be changed
27 | # accordingly using a calibration procedure
28 | x_accel_offset = 1340
29 | y_accel_offset = -150
30 | z_accel_offset = 3085
31 | x_gyro_offset = 86
32 | y_gyro_offset = 19
33 | z_gyro_offset = 74
34 | enable_debug_output = True
35 |
36 | mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset,
37 | z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,
38 | enable_debug_output)
39 |
40 | mpu.dmp_initialize()
41 | mpu.set_DMP_enabled(True)
42 | mpu_int_status = mpu.get_int_status()
43 | print(hex(mpu_int_status))
44 |
45 | packet_size = mpu.DMP_get_FIFO_packet_size()
46 | print(packet_size)
47 | FIFO_count = mpu.get_FIFO_count()
48 | print(FIFO_count)
49 |
50 | count = 0
51 | FIFO_buffer = [0]*64
52 |
53 | FIFO_count_list = list()
54 | while count < 10000:
55 | FIFO_count = mpu.get_FIFO_count()
56 | mpu_int_status = mpu.get_int_status()
57 |
58 | # If overflow is detected by status or fifo count we want to reset
59 | if (FIFO_count == 1024) or (mpu_int_status & 0x10):
60 | mpu.reset_FIFO()
61 | #print('overflow!')
62 | # Check if fifo data is ready
63 | elif (mpu_int_status & 0x02):
64 | # Wait until packet_size number of bytes are ready for reading, default
65 | # is 42 bytes
66 | while FIFO_count < packet_size:
67 | FIFO_count = mpu.get_FIFO_count()
68 | FIFO_buffer = mpu.get_FIFO_bytes(packet_size)
69 | accel = mpu.DMP_get_acceleration_int16(FIFO_buffer)
70 | quat = mpu.DMP_get_quaternion_int16(FIFO_buffer)
71 | grav = mpu.DMP_get_gravity(quat)
72 | roll_pitch_yaw = mpu.DMP_get_euler_roll_pitch_yaw(quat, grav)
73 | if count % 10 == 0:
74 | print('roll: ' + str(roll_pitch_yaw.x))
75 | print('pitch: ' + str(roll_pitch_yaw.y))
76 | print('yaw: ' + str(roll_pitch_yaw.z))
77 | count += 1
78 |
--------------------------------------------------------------------------------
/MPUConstants.py:
--------------------------------------------------------------------------------
1 | __author__ = 'Geir Istad'
2 |
3 | '''
4 | MPU6050 Python I2C Class
5 | Copyright (c) 2015 Geir Istad
6 |
7 | Permission is hereby granted, free of charge, to any person obtaining a copy
8 | of this software and associated documentation files (the "Software"), to deal
9 | in the Software without restriction, including without limitation the rights
10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11 | copies of the Software, and to permit persons to whom the Software is
12 | furnished to do so, subject to the following conditions:
13 |
14 | The above copyright notice and this permission notice shall be included in all
15 | copies or substantial portions of the Software.
16 |
17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23 | SOFTWARE.
24 |
25 |
26 | Code based on I2Cdev library collection - MPU6050 I2C device class
27 | by Jeff Rowberg
28 | ============================================
29 | I2Cdev device library code is placed under the MIT license
30 | Copyright (c) 2012 Jeff Rowberg
31 | Permission is hereby granted, free of charge, to any person obtaining a copy
32 | of this software and associated documentation files (the "Software"), to deal
33 | in the Software without restriction, including without limitation the rights
34 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
35 | copies of the Software, and to permit persons to whom the Software is
36 | furnished to do so, subject to the following conditions:
37 | The above copyright notice and this permission notice shall be included in
38 | all copies or substantial portions of the Software.
39 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
40 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
41 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
42 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
43 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
44 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
45 | THE SOFTWARE.
46 | ===============================================
47 | '''
48 |
49 |
50 | class MPUConstants:
51 | # From MPU6050.h
52 | MPU6050_ADDRESS_AD0_LOW = 0x68 # address pin low (GND), default
53 | MPU6050_ADDRESS_AD0_HIGH = 0x69 # address pin high (VCC)
54 | MPU6050_DEFAULT_ADDRESS = MPU6050_ADDRESS_AD0_LOW
55 |
56 | # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
57 | MPU6050_RA_XG_OFFS_TC = 0x00
58 | # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
59 | MPU6050_RA_YG_OFFS_TC = 0x01
60 | # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
61 | MPU6050_RA_ZG_OFFS_TC = 0x02
62 | # [7:0] X_FINE_GAIN
63 | MPU6050_RA_X_FINE_GAIN = 0x03
64 | # [7:0] Y_FINE_GAIN
65 | MPU6050_RA_Y_FINE_GAIN = 0x04
66 | # [7:0] Z_FINE_GAIN
67 | MPU6050_RA_Z_FINE_GAIN = 0x05
68 | # [15:0] XA_OFFS
69 | MPU6050_RA_XA_OFFS_H = 0x06
70 | MPU6050_RA_XA_OFFS_L_TC = 0x07
71 | # [15:0] YA_OFFS
72 | MPU6050_RA_YA_OFFS_H = 0x08
73 | MPU6050_RA_YA_OFFS_L_TC = 0x09
74 | # [15:0] ZA_OFFS
75 | MPU6050_RA_ZA_OFFS_H = 0x0A
76 | MPU6050_RA_ZA_OFFS_L_TC = 0x0B
77 | # [15:0] XG_OFFS_USR
78 | MPU6050_RA_XG_OFFS_USRH = 0x13
79 | MPU6050_RA_XG_OFFS_USRL = 0x14
80 | # [15:0] YG_OFFS_USR
81 | MPU6050_RA_YG_OFFS_USRH = 0x15
82 | MPU6050_RA_YG_OFFS_USRL = 0x16
83 | # [15:0] ZG_OFFS_USR
84 | MPU6050_RA_ZG_OFFS_USRH = 0x17
85 | MPU6050_RA_ZG_OFFS_USRL = 0x18
86 | MPU6050_RA_SMPLRT_DIV = 0x19
87 | MPU6050_RA_CONFIG = 0x1A
88 | MPU6050_RA_GYRO_CONFIG = 0x1B
89 | MPU6050_RA_ACCEL_CONFIG = 0x1C
90 | MPU6050_RA_FF_THR = 0x1D
91 | MPU6050_RA_FF_DUR = 0x1E
92 | MPU6050_RA_MOT_THR = 0x1F
93 | MPU6050_RA_MOT_DUR = 0x20
94 | MPU6050_RA_ZRMOT_THR = 0x21
95 | MPU6050_RA_ZRMOT_DUR = 0x22
96 | MPU6050_RA_FIFO_EN = 0x23
97 | MPU6050_RA_I2C_MST_CTRL = 0x24
98 | MPU6050_RA_I2C_SLV0_ADDR = 0x25
99 | MPU6050_RA_I2C_SLV0_REG = 0x26
100 | MPU6050_RA_I2C_SLV0_CTRL = 0x27
101 | MPU6050_RA_I2C_SLV1_ADDR = 0x28
102 | MPU6050_RA_I2C_SLV1_REG = 0x29
103 | MPU6050_RA_I2C_SLV1_CTRL = 0x2A
104 | MPU6050_RA_I2C_SLV2_ADDR = 0x2B
105 | MPU6050_RA_I2C_SLV2_REG = 0x2C
106 | MPU6050_RA_I2C_SLV2_CTRL = 0x2D
107 | MPU6050_RA_I2C_SLV3_ADDR = 0x2E
108 | MPU6050_RA_I2C_SLV3_REG = 0x2F
109 | MPU6050_RA_I2C_SLV3_CTRL = 0x30
110 | MPU6050_RA_I2C_SLV4_ADDR = 0x31
111 | MPU6050_RA_I2C_SLV4_REG = 0x32
112 | MPU6050_RA_I2C_SLV4_DO = 0x33
113 | MPU6050_RA_I2C_SLV4_CTRL = 0x34
114 | MPU6050_RA_I2C_SLV4_DI = 0x35
115 | MPU6050_RA_I2C_MST_STATUS = 0x36
116 | MPU6050_RA_INT_PIN_CFG = 0x37
117 | MPU6050_RA_INT_ENABLE = 0x38
118 | MPU6050_RA_DMP_INT_STATUS = 0x39
119 | MPU6050_RA_INT_STATUS = 0x3A
120 | MPU6050_RA_ACCEL_XOUT_H = 0x3B
121 | MPU6050_RA_ACCEL_XOUT_L = 0x3C
122 | MPU6050_RA_ACCEL_YOUT_H = 0x3D
123 | MPU6050_RA_ACCEL_YOUT_L = 0x3E
124 | MPU6050_RA_ACCEL_ZOUT_H = 0x3F
125 | MPU6050_RA_ACCEL_ZOUT_L = 0x40
126 | MPU6050_RA_TEMP_OUT_H = 0x41
127 | MPU6050_RA_TEMP_OUT_L = 0x42
128 | MPU6050_RA_GYRO_XOUT_H = 0x43
129 | MPU6050_RA_GYRO_XOUT_L = 0x44
130 | MPU6050_RA_GYRO_YOUT_H = 0x45
131 | MPU6050_RA_GYRO_YOUT_L = 0x46
132 | MPU6050_RA_GYRO_ZOUT_H = 0x47
133 | MPU6050_RA_GYRO_ZOUT_L = 0x48
134 | MPU6050_RA_EXT_SENS_DATA_00 = 0x49
135 | MPU6050_RA_EXT_SENS_DATA_01 = 0x4A
136 | MPU6050_RA_EXT_SENS_DATA_02 = 0x4B
137 | MPU6050_RA_EXT_SENS_DATA_03 = 0x4C
138 | MPU6050_RA_EXT_SENS_DATA_04 = 0x4D
139 | MPU6050_RA_EXT_SENS_DATA_05 = 0x4E
140 | MPU6050_RA_EXT_SENS_DATA_06 = 0x4F
141 | MPU6050_RA_EXT_SENS_DATA_07 = 0x50
142 | MPU6050_RA_EXT_SENS_DATA_08 = 0x51
143 | MPU6050_RA_EXT_SENS_DATA_09 = 0x52
144 | MPU6050_RA_EXT_SENS_DATA_10 = 0x53
145 | MPU6050_RA_EXT_SENS_DATA_11 = 0x54
146 | MPU6050_RA_EXT_SENS_DATA_12 = 0x55
147 | MPU6050_RA_EXT_SENS_DATA_13 = 0x56
148 | MPU6050_RA_EXT_SENS_DATA_14 = 0x57
149 | MPU6050_RA_EXT_SENS_DATA_15 = 0x58
150 | MPU6050_RA_EXT_SENS_DATA_16 = 0x59
151 | MPU6050_RA_EXT_SENS_DATA_17 = 0x5A
152 | MPU6050_RA_EXT_SENS_DATA_18 = 0x5B
153 | MPU6050_RA_EXT_SENS_DATA_19 = 0x5C
154 | MPU6050_RA_EXT_SENS_DATA_20 = 0x5D
155 | MPU6050_RA_EXT_SENS_DATA_21 = 0x5E
156 | MPU6050_RA_EXT_SENS_DATA_22 = 0x5F
157 | MPU6050_RA_EXT_SENS_DATA_23 = 0x60
158 | MPU6050_RA_MOT_DETECT_STATUS = 0x61
159 | MPU6050_RA_I2C_SLV0_DO = 0x63
160 | MPU6050_RA_I2C_SLV1_DO = 0x64
161 | MPU6050_RA_I2C_SLV2_DO = 0x65
162 | MPU6050_RA_I2C_SLV3_DO = 0x66
163 | MPU6050_RA_I2C_MST_DELAY_CTRL = 0x67
164 | MPU6050_RA_SIGNAL_PATH_RESET = 0x68
165 | MPU6050_RA_MOT_DETECT_CTRL = 0x69
166 | MPU6050_RA_USER_CTRL = 0x6A
167 | MPU6050_RA_PWR_MGMT_1 = 0x6B
168 | MPU6050_RA_PWR_MGMT_2 = 0x6C
169 | MPU6050_RA_BANK_SEL = 0x6D
170 | MPU6050_RA_MEM_START_ADDR = 0x6E
171 | MPU6050_RA_MEM_R_W = 0x6F
172 | MPU6050_RA_DMP_CFG_1 = 0x70
173 | MPU6050_RA_DMP_CFG_2 = 0x71
174 | MPU6050_RA_FIFO_COUNTH = 0x72
175 | MPU6050_RA_FIFO_COUNTL = 0x73
176 | MPU6050_RA_FIFO_R_W = 0x74
177 | MPU6050_RA_WHO_AM_I = 0x75
178 |
179 | MPU6050_TC_PWR_MODE_BIT = 7
180 | MPU6050_TC_OFFSET_BIT = 6
181 | MPU6050_TC_OFFSET_LENGTH = 6
182 | MPU6050_TC_OTP_BNK_VLD_BIT = 0
183 |
184 | MPU6050_VDDIO_LEVEL_VLOGIC = 0
185 | MPU6050_VDDIO_LEVEL_VDD = 1
186 |
187 | MPU6050_CFG_EXT_SYNC_SET_BIT = 5
188 | MPU6050_CFG_EXT_SYNC_SET_LENGTH = 3
189 | MPU6050_CFG_DLPF_CFG_BIT = 2
190 | MPU6050_CFG_DLPF_CFG_LENGTH = 3
191 |
192 | MPU6050_EXT_SYNC_DISABLED = 0x0
193 | MPU6050_EXT_SYNC_TEMP_OUT_L = 0x1
194 | MPU6050_EXT_SYNC_GYRO_XOUT_L = 0x2
195 | MPU6050_EXT_SYNC_GYRO_YOUT_L = 0x3
196 | MPU6050_EXT_SYNC_GYRO_ZOUT_L = 0x4
197 | MPU6050_EXT_SYNC_ACCEL_XOUT_L = 0x5
198 | MPU6050_EXT_SYNC_ACCEL_YOUT_L = 0x6
199 | MPU6050_EXT_SYNC_ACCEL_ZOUT_L = 0x7
200 |
201 | MPU6050_DLPF_BW_256 = 0x00
202 | MPU6050_DLPF_BW_188 = 0x01
203 | MPU6050_DLPF_BW_98 = 0x02
204 | MPU6050_DLPF_BW_42 = 0x03
205 | MPU6050_DLPF_BW_20 = 0x04
206 | MPU6050_DLPF_BW_10 = 0x05
207 | MPU6050_DLPF_BW_5 = 0x06
208 |
209 | MPU6050_GCONFIG_FS_SEL_BIT = 4
210 | MPU6050_GCONFIG_FS_SEL_LENGTH = 2
211 |
212 | MPU6050_GYRO_FS_250 = 0x00
213 | MPU6050_GYRO_FS_500 = 0x01
214 | MPU6050_GYRO_FS_1000 = 0x02
215 | MPU6050_GYRO_FS_2000 = 0x03
216 |
217 | MPU6050_ACONFIG_XA_ST_BIT = 7
218 | MPU6050_ACONFIG_YA_ST_BIT = 6
219 | MPU6050_ACONFIG_ZA_ST_BIT = 5
220 | MPU6050_ACONFIG_AFS_SEL_BIT = 4
221 | MPU6050_ACONFIG_AFS_SEL_LENGTH = 2
222 | MPU6050_ACONFIG_ACCEL_HPF_BIT = 2
223 | MPU6050_ACONFIG_ACCEL_HPF_LENGTH = 3
224 |
225 | MPU6050_ACCEL_FS_2 = 0x00
226 | MPU6050_ACCEL_FS_4 = 0x01
227 | MPU6050_ACCEL_FS_8 = 0x02
228 | MPU6050_ACCEL_FS_16 = 0x03
229 |
230 | MPU6050_DHPF_RESET = 0x00
231 | MPU6050_DHPF_5 = 0x01
232 | MPU6050_DHPF_2P5 = 0x02
233 | MPU6050_DHPF_1P25 = 0x03
234 | MPU6050_DHPF_0P63 = 0x04
235 | MPU6050_DHPF_HOLD = 0x07
236 |
237 | MPU6050_TEMP_FIFO_EN_BIT = 7
238 | MPU6050_XG_FIFO_EN_BIT = 6
239 | MPU6050_YG_FIFO_EN_BIT = 5
240 | MPU6050_ZG_FIFO_EN_BIT = 4
241 | MPU6050_ACCEL_FIFO_EN_BIT = 3
242 | MPU6050_SLV2_FIFO_EN_BIT = 2
243 | MPU6050_SLV1_FIFO_EN_BIT = 1
244 | MPU6050_SLV0_FIFO_EN_BIT = 0
245 |
246 | MPU6050_MULT_MST_EN_BIT = 7
247 | MPU6050_WAIT_FOR_ES_BIT = 6
248 | MPU6050_SLV_3_FIFO_EN_BIT = 5
249 | MPU6050_I2C_MST_P_NSR_BIT = 4
250 | MPU6050_I2C_MST_CLK_BIT = 3
251 | MPU6050_I2C_MST_CLK_LENGTH = 4
252 |
253 | MPU6050_CLOCK_DIV_348 = 0x0
254 | MPU6050_CLOCK_DIV_333 = 0x1
255 | MPU6050_CLOCK_DIV_320 = 0x2
256 | MPU6050_CLOCK_DIV_308 = 0x3
257 | MPU6050_CLOCK_DIV_296 = 0x4
258 | MPU6050_CLOCK_DIV_286 = 0x5
259 | MPU6050_CLOCK_DIV_276 = 0x6
260 | MPU6050_CLOCK_DIV_267 = 0x7
261 | MPU6050_CLOCK_DIV_258 = 0x8
262 | MPU6050_CLOCK_DIV_500 = 0x9
263 | MPU6050_CLOCK_DIV_471 = 0xA
264 | MPU6050_CLOCK_DIV_444 = 0xB
265 | MPU6050_CLOCK_DIV_421 = 0xC
266 | MPU6050_CLOCK_DIV_400 = 0xD
267 | MPU6050_CLOCK_DIV_381 = 0xE
268 | MPU6050_CLOCK_DIV_364 = 0xF
269 |
270 | MPU6050_I2C_SLV_RW_BIT = 7
271 | MPU6050_I2C_SLV_ADDR_BIT = 6
272 | MPU6050_I2C_SLV_ADDR_LENGTH = 7
273 | MPU6050_I2C_SLV_EN_BIT = 7
274 | MPU6050_I2C_SLV_BYTE_SW_BIT = 6
275 | MPU6050_I2C_SLV_REG_DIS_BIT = 5
276 | MPU6050_I2C_SLV_GRP_BIT = 4
277 | MPU6050_I2C_SLV_LEN_BIT = 3
278 | MPU6050_I2C_SLV_LEN_LENGTH = 4
279 |
280 | MPU6050_I2C_SLV4_RW_BIT = 7
281 | MPU6050_I2C_SLV4_ADDR_BIT = 6
282 | MPU6050_I2C_SLV4_ADDR_LENGTH = 7
283 | MPU6050_I2C_SLV4_EN_BIT = 7
284 | MPU6050_I2C_SLV4_INT_EN_BIT = 6
285 | MPU6050_I2C_SLV4_REG_DIS_BIT = 5
286 | MPU6050_I2C_SLV4_MST_DLY_BIT = 4
287 | MPU6050_I2C_SLV4_MST_DLY_LENGTH = 5
288 |
289 | MPU6050_MST_PASS_THROUGH_BIT = 7
290 | MPU6050_MST_I2C_SLV4_DONE_BIT = 6
291 | MPU6050_MST_I2C_LOST_ARB_BIT = 5
292 | MPU6050_MST_I2C_SLV4_NACK_BIT = 4
293 | MPU6050_MST_I2C_SLV3_NACK_BIT = 3
294 | MPU6050_MST_I2C_SLV2_NACK_BIT = 2
295 | MPU6050_MST_I2C_SLV1_NACK_BIT = 1
296 | MPU6050_MST_I2C_SLV0_NACK_BIT = 0
297 |
298 | MPU6050_INTCFG_INT_LEVEL_BIT = 7
299 | MPU6050_INTCFG_INT_OPEN_BIT = 6
300 | MPU6050_INTCFG_LATCH_INT_EN_BIT = 5
301 | MPU6050_INTCFG_INT_RD_CLEAR_BIT = 4
302 | MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT = 3
303 | MPU6050_INTCFG_FSYNC_INT_EN_BIT = 2
304 | MPU6050_INTCFG_I2C_BYPASS_EN_BIT = 1
305 | MPU6050_INTCFG_CLKOUT_EN_BIT = 0
306 |
307 | MPU6050_INTMODE_ACTIVEHIGH = 0x00
308 | MPU6050_INTMODE_ACTIVELOW = 0x01
309 |
310 | MPU6050_INTDRV_PUSHPULL = 0x00
311 | MPU6050_INTDRV_OPENDRAIN = 0x01
312 |
313 | MPU6050_INTLATCH_50USPULSE = 0x00
314 | MPU6050_INTLATCH_WAITCLEAR = 0x01
315 |
316 | MPU6050_INTCLEAR_STATUSREAD = 0x00
317 | MPU6050_INTCLEAR_ANYREAD = 0x01
318 |
319 | MPU6050_INTERRUPT_FF_BIT = 7
320 | MPU6050_INTERRUPT_MOT_BIT = 6
321 | MPU6050_INTERRUPT_ZMOT_BIT = 5
322 | MPU6050_INTERRUPT_FIFO_OFLOW_BIT = 4
323 | MPU6050_INTERRUPT_I2C_MST_INT_BIT = 3
324 | MPU6050_INTERRUPT_PLL_RDY_INT_BIT = 2
325 | MPU6050_INTERRUPT_DMP_INT_BIT = 1
326 | MPU6050_INTERRUPT_DATA_RDY_BIT = 0
327 |
328 | # TODO: figure out what these actually do
329 | # UMPL source code is not very obivous
330 | MPU6050_DMPINT_5_BIT = 5
331 | MPU6050_DMPINT_4_BIT = 4
332 | MPU6050_DMPINT_3_BIT = 3
333 | MPU6050_DMPINT_2_BIT = 2
334 | MPU6050_DMPINT_1_BIT = 1
335 | MPU6050_DMPINT_0_BIT = 0
336 |
337 | MPU6050_MOTION_MOT_XNEG_BIT = 7
338 | MPU6050_MOTION_MOT_XPOS_BIT = 6
339 | MPU6050_MOTION_MOT_YNEG_BIT = 5
340 | MPU6050_MOTION_MOT_YPOS_BIT = 4
341 | MPU6050_MOTION_MOT_ZNEG_BIT = 3
342 | MPU6050_MOTION_MOT_ZPOS_BIT = 2
343 | MPU6050_MOTION_MOT_ZRMOT_BIT = 0
344 |
345 | MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT = 7
346 | MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT = 4
347 | MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT = 3
348 | MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT = 2
349 | MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT = 1
350 | MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT = 0
351 |
352 | MPU6050_PATHRESET_GYRO_RESET_BIT = 2
353 | MPU6050_PATHRESET_ACCEL_RESET_BIT = 1
354 | MPU6050_PATHRESET_TEMP_RESET_BIT = 0
355 |
356 | MPU6050_DETECT_ACCEL_ON_DELAY_BIT = 5
357 | MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH = 2
358 | MPU6050_DETECT_FF_COUNT_BIT = 3
359 | MPU6050_DETECT_FF_COUNT_LENGTH = 2
360 | MPU6050_DETECT_MOT_COUNT_BIT = 1
361 | MPU6050_DETECT_MOT_COUNT_LENGTH = 2
362 |
363 | MPU6050_DETECT_DECREMENT_RESET = 0x0
364 | MPU6050_DETECT_DECREMENT_1 = 0x1
365 | MPU6050_DETECT_DECREMENT_2 = 0x2
366 | MPU6050_DETECT_DECREMENT_4 = 0x3
367 |
368 | MPU6050_USERCTRL_DMP_EN_BIT = 7
369 | MPU6050_USERCTRL_FIFO_EN_BIT = 6
370 | MPU6050_USERCTRL_I2C_MST_EN_BIT = 5
371 | MPU6050_USERCTRL_I2C_IF_DIS_BIT = 4
372 | MPU6050_USERCTRL_DMP_RESET_BIT = 3
373 | MPU6050_USERCTRL_FIFO_RESET_BIT = 2
374 | MPU6050_USERCTRL_I2C_MST_RESET_BIT = 1
375 | MPU6050_USERCTRL_SIG_COND_RESET_BIT = 0
376 |
377 | MPU6050_PWR1_DEVICE_RESET_BIT = 7
378 | MPU6050_PWR1_SLEEP_BIT = 6
379 | MPU6050_PWR1_CYCLE_BIT = 5
380 | MPU6050_PWR1_TEMP_DIS_BIT = 3
381 | MPU6050_PWR1_CLKSEL_BIT = 2
382 | MPU6050_PWR1_CLKSEL_LENGTH = 3
383 |
384 | MPU6050_CLOCK_INTERNAL = 0x00
385 | MPU6050_CLOCK_PLL_XGYRO = 0x01
386 | MPU6050_CLOCK_PLL_YGYRO = 0x02
387 | MPU6050_CLOCK_PLL_ZGYRO = 0x03
388 | MPU6050_CLOCK_PLL_EXT32K = 0x04
389 | MPU6050_CLOCK_PLL_EXT19M = 0x05
390 | MPU6050_CLOCK_KEEP_RESET = 0x07
391 |
392 | MPU6050_PWR2_LP_WAKE_CTRL_BIT = 7
393 | MPU6050_PWR2_LP_WAKE_CTRL_LENGTH = 2
394 | MPU6050_PWR2_STBY_XA_BIT = 5
395 | MPU6050_PWR2_STBY_YA_BIT = 4
396 | MPU6050_PWR2_STBY_ZA_BIT = 3
397 | MPU6050_PWR2_STBY_XG_BIT = 2
398 | MPU6050_PWR2_STBY_YG_BIT = 1
399 | MPU6050_PWR2_STBY_ZG_BIT = 0
400 |
401 | MPU6050_WAKE_FREQ_1P25 = 0x0
402 | MPU6050_WAKE_FREQ_2P5 = 0x1
403 | MPU6050_WAKE_FREQ_5 = 0x2
404 | MPU6050_WAKE_FREQ_10 = 0x3
405 |
406 | MPU6050_BANKSEL_PRFTCH_EN_BIT = 6
407 | MPU6050_BANKSEL_CFG_USER_BANK_BIT = 5
408 | MPU6050_BANKSEL_MEM_SEL_BIT = 4
409 | MPU6050_BANKSEL_MEM_SEL_LENGTH = 5
410 |
411 | MPU6050_WHO_AM_I_BIT = 6
412 | MPU6050_WHO_AM_I_LENGTH = 6
413 |
414 | MPU6050_DMP_MEMORY_BANKS = 8
415 | MPU6050_DMP_MEMORY_BANK_SIZE = 256
416 | MPU6050_DMP_MEMORY_CHUNK_SIZE = 16
417 |
418 | # From MPU6050_6Axis_MotionApps20.h
419 | MPU6050_DMP_CODE_SIZE = 1929 # dmpMemory[]
420 | MPU6050_DMP_CONFIG_SIZE = 192 # dmpConfig[]
421 | MPU6050_DMP_UPDATES_SIZE = 47 # dmpUpdates[]
422 | '''
423 | * ================================================================================================ *
424 | | Default MotionApps v2.0 42-byte FIFO packet structure: |
425 | | |
426 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
427 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
428 | | |
429 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
430 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
431 | * ================================================================================================ *
432 | '''
433 | # dmpMemory has size MPU6050_DMP_CODE_SIZE = 1929
434 | dmpMemory = [
435 | # bank 0, 256 bytes
436 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02,
437 | 0x00, 0x03, 0x00, 0x00,
438 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B,
439 | 0x12, 0x82, 0x00, 0x01,
440 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
441 | 0x00, 0x00, 0x00, 0x00,
442 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72,
443 | 0x00, 0x00, 0x00, 0x00,
444 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF,
445 | 0xFF, 0xFE, 0x80, 0x01,
446 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
447 | 0x00, 0x00, 0x00, 0x00,
448 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09,
449 | 0x3E, 0x80, 0x00, 0x00,
450 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
451 | 0x60, 0x00, 0x00, 0x00,
452 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55,
453 | 0x00, 0x00, 0x21, 0x82,
454 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00,
455 | 0x00, 0x05, 0x80, 0x00,
456 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
457 | 0x00, 0x03, 0x00, 0x00,
458 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32,
459 | 0x00, 0x00, 0x5E, 0xC0,
460 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
461 | 0x00, 0x00, 0x00, 0x00,
462 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B,
463 | 0x00, 0x6C, 0x12, 0xCC,
464 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00,
465 | 0x40, 0x00, 0x01, 0xF4,
466 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6,
467 | 0x00, 0x00, 0x27, 0x10,
468 |
469 | # bank 1, 256 bytes
470 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
471 | 0x00, 0x00, 0x00, 0x00,
472 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00,
473 | 0x01, 0x00, 0x00, 0x00,
474 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0,
475 | 0xFF, 0xD9, 0x5B, 0xC8,
476 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2,
477 | 0x00, 0xCE, 0xBB, 0xF7,
478 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02,
479 | 0x02, 0x00, 0x00, 0x0C,
480 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00,
481 | 0x40, 0x00, 0x00, 0x00,
482 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00,
483 | 0x00, 0x00, 0x00, 0x14,
484 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
485 | 0x00, 0x00, 0x00, 0x00,
486 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
487 | 0x00, 0x00, 0x00, 0x00,
488 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
489 | 0x00, 0x00, 0x00, 0x00,
490 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC,
491 | 0xC6, 0x7E, 0xD1, 0x6C,
492 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A,
493 | 0x00, 0x00, 0x00, 0x00,
494 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0,
495 | 0x00, 0x00, 0x00, 0x30,
496 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
497 | 0x00, 0x00, 0x00, 0x00,
498 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
499 | 0x00, 0x00, 0x00, 0x00,
500 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE,
501 | 0x00, 0x0C, 0x02, 0xD0,
502 |
503 | # bank 2, 256 bytes
504 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00,
505 | 0x00, 0x00, 0x00, 0x00,
506 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00,
507 | 0x00, 0x00, 0x01, 0x00,
508 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00,
509 | 0xFF, 0xEF, 0x00, 0x00,
510 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
511 | 0x00, 0x00, 0x00, 0x00,
512 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
513 | 0x00, 0x00, 0x00, 0x00,
514 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
515 | 0x00, 0x00, 0x00, 0x00,
516 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
517 | 0x00, 0x00, 0x00, 0x00,
518 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
519 | 0x00, 0x00, 0x00, 0x00,
520 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
521 | 0x00, 0x00, 0x00, 0x00,
522 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
523 | 0x00, 0x00, 0x00, 0x00,
524 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
525 | 0x00, 0x00, 0x00, 0x00,
526 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
527 | 0x40, 0x00, 0x00, 0x00,
528 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
529 | 0x00, 0x00, 0x00, 0x00,
530 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
531 | 0x00, 0x00, 0x00, 0x00,
532 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
533 | 0x00, 0x00, 0x00, 0x00,
534 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
535 | 0x00, 0x00, 0x00, 0x00,
536 |
537 | # bank 3, 256 bytes
538 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91,
539 | 0xF7, 0x4A, 0x90, 0x7F,
540 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2,
541 | 0xCE, 0x81, 0xF3, 0xC2,
542 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8,
543 | 0x80, 0xBA, 0xA7, 0xDF,
544 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94,
545 | 0x24, 0x48, 0x70, 0x3C,
546 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D,
547 | 0x55, 0x7D, 0xD8, 0xB1,
548 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C,
549 | 0x80, 0xA8, 0xF1, 0x01,
550 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2,
551 | 0x0E, 0xB8, 0x97, 0x80,
552 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5,
553 | 0xCD, 0xC7, 0xA9, 0x0C,
554 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66,
555 | 0xB0, 0xB4, 0xBA, 0x80,
556 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00,
557 | 0x89, 0x0E, 0x16, 0x1E,
558 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36,
559 | 0x56, 0x76, 0xF1, 0xB9,
560 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98,
561 | 0xB9, 0xAF, 0xF0, 0x24,
562 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83,
563 | 0xB5, 0x93, 0xAF, 0xF0,
564 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF,
565 | 0xD9, 0xFA, 0xA3, 0x86,
566 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5,
567 | 0xC7, 0xB2, 0x8C, 0xC1,
568 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1,
569 | 0xB8, 0xA8, 0xB2, 0x86,
570 |
571 | # bank 4, 256 bytes
572 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35,
573 | 0x3D, 0xB2, 0xB6, 0xBA,
574 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A,
575 | 0xB8, 0xAA, 0x87, 0x2C,
576 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4,
577 | 0xCD, 0xC9, 0xF1, 0xB8,
578 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55,
579 | 0x7D, 0xB5, 0x93, 0xA3,
580 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97,
581 | 0x83, 0xA8, 0x11, 0x84,
582 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64,
583 | 0x18, 0xD8, 0xF1, 0xA5,
584 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A,
585 | 0x40, 0x48, 0xF9, 0xF3,
586 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18,
587 | 0x97, 0x82, 0xA8, 0xF1,
588 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3,
589 | 0xDE, 0xD8, 0x83, 0xA5,
590 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1,
591 | 0x84, 0x92, 0xA2, 0x4D,
592 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32,
593 | 0xD8, 0x50, 0x71, 0xD9,
594 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8,
595 | 0x78, 0x93, 0xA3, 0x4D,
596 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32,
597 | 0xD8, 0x50, 0x71, 0xD9,
598 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8,
599 | 0x78, 0xA8, 0x8A, 0x9A,
600 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98,
601 | 0xA8, 0xD9, 0x08, 0xD8,
602 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1,
603 | 0xAF, 0xC8, 0x97, 0x87,
604 |
605 | # bank 5, 256 bytes
606 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D,
607 | 0xF3, 0xD9, 0x2A, 0xD8,
608 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9,
609 | 0x61, 0xD8, 0x6C, 0x68,
610 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3,
611 | 0x84, 0x19, 0x3D, 0x5D,
612 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8,
613 | 0xB0, 0xAF, 0x8F, 0x94,
614 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8,
615 | 0x87, 0x9A, 0x35, 0xDA,
616 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98,
617 | 0x85, 0x02, 0x2E, 0x56,
618 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9,
619 | 0x28, 0xD8, 0x4D, 0xD9,
620 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8,
621 | 0xA3, 0x29, 0x83, 0xDA,
622 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8,
623 | 0xB8, 0xB0, 0xA8, 0x8A,
624 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA,
625 | 0xDE, 0xD8, 0xA8, 0x60,
626 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06,
627 | 0x99, 0x07, 0xAB, 0x97,
628 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8,
629 | 0x8C, 0x9C, 0xF0, 0x04,
630 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98,
631 | 0x2C, 0x50, 0x50, 0x78,
632 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79,
633 | 0x8B, 0x29, 0x51, 0x79,
634 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38,
635 | 0x8B, 0x39, 0x40, 0x68,
636 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09,
637 | 0x71, 0x58, 0x44, 0x68,
638 |
639 | # bank 6, 256 bytes
640 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C,
641 | 0xF0, 0x8C, 0xA8, 0x04,
642 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C,
643 | 0x02, 0x26, 0x46, 0x66,
644 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69,
645 | 0x38, 0x64, 0x48, 0x31,
646 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8,
647 | 0x19, 0x31, 0x48, 0x60,
648 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19,
649 | 0x86, 0xA8, 0x6E, 0x76,
650 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22,
651 | 0x8A, 0x6E, 0x8A, 0x56,
652 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E,
653 | 0x6E, 0x9D, 0xB8, 0xAD,
654 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D,
655 | 0x55, 0x7D, 0x81, 0x91,
656 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8,
657 | 0x4D, 0xD9, 0x48, 0xD8,
658 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8,
659 | 0x51, 0xD9, 0x04, 0xAE,
660 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE,
661 | 0x19, 0x81, 0xAD, 0xD9,
662 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7,
663 | 0xD9, 0xAD, 0xAD, 0xAD,
664 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E,
665 | 0x76, 0xF3, 0xAC, 0x2E,
666 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C,
667 | 0xAC, 0x30, 0x18, 0xA8,
668 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C,
669 | 0x24, 0xF2, 0xB0, 0x89,
670 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51,
671 | 0xD9, 0xD8, 0xD8, 0x79,
672 |
673 | # bank 7, 138 bytes (remainder)
674 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91,
675 | 0x2D, 0xD9, 0x28, 0xD8,
676 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35,
677 | 0x3D, 0x80, 0x25, 0xDA,
678 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28,
679 | 0x34, 0x3C, 0xF3, 0xAB,
680 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0,
681 | 0x87, 0x9C, 0xB9, 0xA3,
682 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D,
683 | 0xF1, 0xA3, 0xA3, 0xA3,
684 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
685 | 0xA3, 0xA3, 0xA3, 0xA3,
686 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99,
687 | 0xF1, 0xA3, 0xA3, 0xA3,
688 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3,
689 | 0x9B, 0xA3, 0xA3, 0xDC,
690 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF]
691 |
692 | # dmpConfig has size MPU6050_DMP_CONFIG_SIZE = 192
693 | dmpConfig = [
694 | # BANK OFFSET LENGTH [DATA]
695 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, # FCFG_1 inv_set_gyro_calibration
696 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, # FCFG_3 inv_set_gyro_calibration
697 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2,
698 | # D_0_104 inv_set_gyro_calibration
699 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1,
700 | # D_0_24 inv_set_gyro_calibration
701 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00,
702 | # D_1_152 inv_set_accel_calibration
703 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
704 | # FCFG_2 inv_set_accel_calibration
705 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, # FCFG_7 inv_set_accel_calibration
706 | 0x00, 0x6C, 0x02, 0x20, 0x00, # D_0_108 inv_set_accel_calibration
707 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00,
708 | # CPASS_MTX_00 inv_set_compass_calibration
709 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_01
710 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_02
711 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_10
712 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_11
713 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_12
714 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_20
715 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_21
716 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, # CPASS_MTX_22
717 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00,
718 | # D_1_236 inv_apply_endian_accel
719 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97,
720 | # FCFG_2 inv_set_mpu_sensors
721 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D,
722 | # CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
723 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, # FCFG_5 inv_set_bias_update
724 | 0x00, 0xA3, 0x01, 0x00, # D_0_163 inv_set_dead_zone
725 | # SPECIAL 0x01 = enable interrupts
726 | 0x00, 0x00, 0x00, 0x01, # SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
727 | 0x07, 0x86, 0x01, 0xFE, # CFG_6 inv_set_fifo_interupt
728 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38,
729 | # CFG_8 inv_send_quaternion
730 | 0x07, 0x7E, 0x01, 0x30, # CFG_16 inv_set_footer
731 | 0x07, 0x46, 0x01, 0x9A, # CFG_GYRO_SOURCE inv_send_gyro
732 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38,
733 | # CFG_9 inv_send_gyro -> inv_construct3_fifo
734 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38,
735 | # CFG_12 inv_send_accel -> inv_construct3_fifo
736 | 0x02, 0x16, 0x02, 0x00, 0x01] # D_0_22 inv_set_fifo_rate
737 |
738 | # This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz.
739 | # 0x07 is 25 Hz, 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends
740 | # to result in very noisy data. DMP output frequency is calculated easily
741 | # using this equation: (200Hz / (1 + value))
742 |
743 | # It is important to make sure the host processor can keep up with reading
744 | # and processing the FIFO output at the desired rate. Handling FIFO overflow
745 | # cleanly is also a good idea.
746 |
747 | # dmpUpdates has size MPU6050_DMP_UPDATES_SIZE = 47
748 | dmpUpdates = [
749 | 0x01, 0xB2, 0x02, 0xFF, 0xFF,
750 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
751 | 0x01, 0x6A, 0x02, 0x06, 0x00,
752 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
753 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
754 | 0x01, 0x62, 0x02, 0x00, 0x00,
755 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00]
756 |
757 |
--------------------------------------------------------------------------------
/Quaternion.py:
--------------------------------------------------------------------------------
1 | __author__ = 'Geir Istad'
2 | """
3 | MPU6050 Python I2C Class
4 | Copyright (c) 2015 Geir Istad
5 |
6 | Permission is hereby granted, free of charge, to any person obtaining a copy
7 | of this software and associated documentation files (the "Software"), to deal
8 | in the Software without restriction, including without limitation the rights
9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 | copies of the Software, and to permit persons to whom the Software is
11 | furnished to do so, subject to the following conditions:
12 |
13 | The above copyright notice and this permission notice shall be included in all
14 | copies or substantial portions of the Software.
15 |
16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 | SOFTWARE.
23 |
24 |
25 | Code based on
26 | I2Cdev library collection - 3D math helper
27 | by Jeff Rowberg
28 | ============================================
29 | I2Cdev device library code is placed under the MIT license
30 | Copyright (c) 2012 Jeff Rowberg
31 | Permission is hereby granted, free of charge, to any person obtaining a copy
32 | of this software and associated documentation files (the "Software"), to deal
33 | in the Software without restriction, including without limitation the rights
34 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
35 | copies of the Software, and to permit persons to whom the Software is
36 | furnished to do so, subject to the following conditions:
37 | The above copyright notice and this permission notice shall be included in
38 | all copies or substantial portions of the Software.
39 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
40 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
41 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
42 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
43 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
44 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
45 | THE SOFTWARE.
46 | ===============================================
47 | """
48 | from math import sqrt
49 |
50 |
51 | class Quaternion:
52 | w = 0.0
53 | x = 0.0
54 | y = 0.0
55 | z = 0.0
56 |
57 | def __init__(self, a_w=1.0, a_x=0.0, a_y=0.0, a_z=0.0):
58 | self.w = a_w
59 | self.x = a_x
60 | self.y = a_y
61 | self.z = a_z
62 |
63 | def get_product(self, a_quat):
64 | result = Quaternion(
65 | self.w * a_quat.w - self.x * a_quat.x -
66 | self.y * a_quat.y - self.z * a_quat.z,
67 |
68 | self.w * a_quat.x + self.x * a_quat.w +
69 | self.y * a_quat.z - self.z * a_quat.y,
70 |
71 | self.w * a_quat.y - self.x * a_quat.z +
72 | self.y * a_quat.w + self.z * a_quat.x,
73 |
74 | self.w * a_quat.z + self.x * a_quat.y -
75 | self.y * a_quat.x + self.z * a_quat.w)
76 | return result
77 |
78 | def get_conjugate(self):
79 | result = Quaternion(self.w, -self.x, -self.y, -self.z)
80 | return result
81 |
82 | def get_magnitude(self):
83 | return sqrt(self.w * self.w + self.x * self.x + self.y * self.y +
84 | self.z * self.z)
85 |
86 | def normalize(self):
87 | m = self.get_magnitude()
88 | self.w = self.w / m
89 | self.x = self.x / m
90 | self.y = self.y / m
91 | self.z = self.z / m
92 |
93 | def get_normalized(self):
94 | result = Quaternion(self.w, self.x, self.y, self.z)
95 | result.normalize()
96 | return result
97 |
98 |
99 | class XYZVector:
100 | x = 0.0
101 | y = 0.0
102 | z = 0.0
103 |
104 | def __init__(self, a_x=0.0, a_y=0.0, a_z=0.0):
105 | self.x = a_x
106 | self.y = a_y
107 | self.z = a_z
108 |
109 | def get_magnitude(self):
110 | return sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
111 |
112 | def normalize(self):
113 | m = self.get_magnitude()
114 | self.x = self.x / m
115 | self.y = self.y / m
116 | self.z = self.z / m
117 |
118 | def get_normalized(self):
119 | result = XYZVector(self.x, self.y, self.z)
120 | result.normalize()
121 | return result
122 |
123 | def rotate(self, a_quat):
124 | p = Quaternion(0.0, self.x, self.y, self.z)
125 | p = a_quat.get_product(p)
126 | p = p.get_product(a_quat.get_conjugate())
127 | # By magic quaternion p is now [0, x', y', z']
128 | self.x = p.x
129 | self.y = p.y
130 | self.z = p.z
131 |
132 | def get_rotated(self, a_quat):
133 | r = XYZVector(self.x, self.y, self.z)
134 | r.rotate(a_quat)
135 | return r
136 |
137 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # mpu6050_DMP_python
2 | attempt to port DMP module library to raspberry pico
3 |
4 | based on:
5 | https://github.com/thisisG/MPU6050-I2C-Python-Class
6 |
7 |
8 | This is an experiment to port existing librarty from python to micropython raspberry pi board.
9 |
10 | DMP should in theory provide better datat than reading from Gyro and accel registers directly.
11 | It also uses FIFO.
12 |
13 |
--------------------------------------------------------------------------------
/SimplePID.py:
--------------------------------------------------------------------------------
1 | __author__ = 'Geir Istad'
2 | """
3 | The MIT License (MIT)
4 |
5 | SimplePID - PID in Python made easy!
6 | Copyright (c) 2015 Geir Istad
7 |
8 | Permission is hereby granted, free of charge, to any person obtaining a copy
9 | of this software and associated documentation files (the "Software"), to deal
10 | in the Software without restriction, including without limitation the rights
11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 | copies of the Software, and to permit persons to whom the Software is
13 | furnished to do so, subject to the following conditions:
14 |
15 | The above copyright notice and this permission notice shall be included in all
16 | copies or substantial portions of the Software.
17 |
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24 | SOFTWARE.
25 | """
26 | import utime
27 |
28 |
29 | class SimplePID():
30 | __last_time_ms = 0.0
31 | __delta_time_ms = 0.0
32 | __kp = 0.0
33 | __ki = 0.0
34 | __kd = 0.0
35 | __set_point = 0.0
36 | __output_value_min_1 = 0.0
37 | __error_min_1 = 0.0
38 | __error_min_2 = 0.0
39 | __coeff_a = 0.0
40 | __coeff_b = 0.0
41 | __coeff_c = 0.0
42 | __min_output = 0.0
43 | __max_output = 0.0
44 | __PID_direction_direct = True
45 |
46 | def __init__(self, a_set_point, a_min_output, a_max_output, a_kp, a_ki,
47 | a_kd, a_delta_time_ms=100, a_direction_direct=True):
48 | self.__last_time_ms = utime.ticks_ms()
49 | self.__set_point = a_set_point
50 | self.__min_output = a_min_output
51 | self.__max_output = a_max_output
52 | self.__kp = a_kp
53 | self.__ki = a_ki
54 | self.__kd = a_kd
55 | self.__delta_time_ms = a_delta_time_ms
56 | self.__PID_direction_direct = a_direction_direct
57 | self.__update_coeffs()
58 |
59 | def get_controller_direction(self):
60 | return self.__PID_direction_direct
61 |
62 | def set_controller_direction(self, a_direction_direct):
63 | self.__PID_direction_direct = a_direction_direct
64 | self.__update_coeffs()
65 |
66 | def get_delta_time_ms(self):
67 | return self.__delta_time
68 |
69 | def set_delta_time_ms(self, a_delta_time_ms):
70 | self.__delta_time_ms = a_delta_time_ms
71 | self.__update_coeffs()
72 |
73 | def get_kp(self):
74 | return self.__kp
75 |
76 | def get_ki(self):
77 | return self.__ki
78 |
79 | def get_kd(self):
80 | return self.__kd
81 |
82 | def set_kp(self, a_kp):
83 | self.__kp = a_kp
84 | self.__update_coeffs()
85 |
86 | def set_ki(self, a_ki):
87 | self.__ki = a_ki
88 | self.__update_coeffs()
89 |
90 | def set_kd(self, a_kd):
91 | self.__kd = a_kd
92 | self.__update_coeffs()
93 |
94 | def check_time(self):
95 | current_time_ms = 1000*(utime.ticks_ms())
96 | if current_time_ms - self.__last_time_ms > self.__delta_time_ms:
97 | self.__last_time_ms = current_time_ms
98 | return True
99 | else:
100 | return False
101 |
102 | def get_output_value(self, a_feedback_value):
103 | current_error = self.__set_point - a_feedback_value
104 | factor_1 = self.__output_value_min_1
105 | factor_2 = self.__coeff_a * current_error
106 | factor_3 = self.__coeff_b * self.__error_min_1
107 | factor_4 = self.__coeff_c * self.__error_min_2
108 | output_value = factor_1 + factor_2 + factor_3 + factor_4
109 |
110 | if output_value > self.__max_output:
111 | output_value = self.__max_output
112 | elif output_value < self.__min_output:
113 | output_value = self.__min_output
114 |
115 | self.__error_min_2 = self.__error_min_1
116 | self.__error_min_1 = current_error
117 | self.__output_value_min_1 = output_value
118 |
119 | return output_value
120 |
121 | def __update_coeffs(self):
122 | self.__update_coeff_a()
123 | self.__update_coeff_b()
124 | self.__update_coeff_c()
125 |
126 | def __update_coeff_a(self):
127 | if self.__PID_direction_direct is True:
128 | kp = float(self.__kp)
129 | ki = float(self.__ki)
130 | kd = float(self.__kd)
131 | else:
132 | kp = 0.0 - self.__kp
133 | ki = 0.0 - self.__ki
134 | kd = 0.0 - self.__kd
135 | factor_1 = kp
136 | factor_2 = ki * (self.__delta_time_ms / (1000.0 * 2.0))
137 | factor_3 = kd / (self.__delta_time_ms / 1000.0)
138 | self.__coeff_a = factor_1 + factor_2 + factor_3
139 |
140 | def __update_coeff_b(self):
141 | if self.__PID_direction_direct is True:
142 | kp = float(self.__kp)
143 | ki = float(self.__ki)
144 | kd = float(self.__kd)
145 | else:
146 | kp = 0.0 - self.__kp
147 | ki = 0.0 - self.__ki
148 | kd = 0.0 - self.__kd
149 | factor_1 = - kp
150 | factor_2 = ki * (self.__delta_time_ms / (1000.0 * 2.0))
151 | factor_3 = - ((2.0 * kd) / (self.__delta_time_ms / 1000.0))
152 | self.__coeff_b = factor_1 + factor_2 + factor_3
153 |
154 | def __update_coeff_c(self):
155 | if self.__PID_direction_direct is True:
156 | kd = self.__kd
157 | else:
158 | kd = 0.0 - self.__kd
159 | self.__coeff_c = kd / (self.__delta_time_ms / 1000.0)
160 |
161 |
--------------------------------------------------------------------------------
/csv.py:
--------------------------------------------------------------------------------
1 | QUOTE_MINIMAL = ';'
2 |
3 |
4 | class WriterMock:
5 | def writerow(self, data):
6 | print(data)
7 |
8 |
9 | def writer(log_file, delimiter=',', quotechar='|', quoting=';'):
10 | return WriterMock()
11 |
--------------------------------------------------------------------------------
/usmbus.py:
--------------------------------------------------------------------------------
1 | """ Provides an SMBus class for use on micropython """
2 |
3 | try:
4 | from machine import I2C
5 | except ImportError:
6 | raise ImportError("Can't find the micropython machine.I2C class: "
7 | "perhaps you don't need this adapter?")
8 |
9 |
10 | class SMBus(I2C):
11 | """ Provides an 'SMBus' module which supports some of the py-smbus
12 | i2c methods, as well as being a subclass of machine.I2C
13 |
14 | Hopefully this will allow you to run code that was targeted at
15 | py-smbus unmodified on micropython.
16 |
17 | Use it like you would the machine.I2C class:
18 |
19 | import usmbus.SMBus
20 |
21 | bus = SMBus(1, pins=('G15','G10'), baudrate=100000)
22 | bus.read_byte_data(addr, register)
23 | ... etc
24 | """
25 |
26 | def read_byte_data(self, addr, register):
27 | """ Read a single byte from register of device at addr
28 | Returns a single byte """
29 | return self.readfrom_mem(addr, register, 1)[0]
30 |
31 | def read_i2c_block_data(self, addr, register, length):
32 | """ Read a block of length from register of device at addr
33 | Returns a bytes object filled with whatever was read """
34 | return self.readfrom_mem(addr, register, length)
35 |
36 | def write_byte_data(self, addr, register, data):
37 | """ Write a single byte from buffer `data` to register of device at addr
38 | Returns None """
39 | # writeto_mem() expects something it can treat as a buffer
40 | if isinstance(data, int):
41 | data = bytes([data])
42 | return self.writeto_mem(addr, register, data)
43 |
44 | def write_i2c_block_data(self, addr, register, data):
45 | """ Write multiple bytes of data to register of device at addr
46 | Returns None """
47 | # writeto_mem() expects something it can treat as a buffer
48 | if isinstance(data, int):
49 | data = bytes([data])
50 | return self.writeto_mem(addr, register, data)
51 |
52 | # The follwing haven't been implemented, but could be.
53 | def read_byte(self, *args, **kwargs):
54 | """ Not yet implemented """
55 | raise RuntimeError("Not yet implemented")
56 |
57 | def write_byte(self, *args, **kwargs):
58 | """ Not yet implemented """
59 | raise RuntimeError("Not yet implemented")
60 |
61 | def read_word_data(self, *args, **kwargs):
62 | """ Not yet implemented """
63 | raise RuntimeError("Not yet implemented")
64 |
65 | def write_word_data(self, *args, **kwargs):
66 | """ Not yet implemented """
67 | raise RuntimeError("Not yet implemented")
68 |
69 |
70 |
71 |
--------------------------------------------------------------------------------