├── .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 | 6 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | --------------------------------------------------------------------------------