├── .gitignore ├── MPU6050_IRQ_example.py ├── LICENSE ├── bspm_P9_11_27-00A0.dts ├── testMPU.py ├── MPU6050_example.py ├── Quaternion.py ├── MPU6050_cal.py ├── MPUConstants.py └── MPU6050.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | -------------------------------------------------------------------------------- /MPU6050_IRQ_example.py: -------------------------------------------------------------------------------- 1 | import time 2 | from MPU6050 import MPU6050 3 | from MPU6050 import MPU6050IRQHandler 4 | import Adafruit_BBIO.GPIO as GPIO 5 | 6 | i2c_bus = 1 7 | device_address = 0x68 8 | # The offsets are different for each device and should be changed to 9 | # sutiable figures using a calibration procedure 10 | x_accel_offset = -5489 11 | y_accel_offset = -1441 12 | z_accel_offset = 1305 13 | x_gyro_offset = -2 14 | y_gyro_offset = -72 15 | z_gyro_offset = -5 16 | enable_debug_output = True 17 | enable_logging = True 18 | log_file = 'mpulog.csv' 19 | 20 | mpu = MPU6050(i2c_bus, device_address, x_accel_offset, 21 | y_accel_offset, z_accel_offset, x_gyro_offset, 22 | y_gyro_offset, z_gyro_offset, enable_debug_output) 23 | mpuC = MPU6050IRQHandler(mpu, enable_logging, log_file) 24 | 25 | GPIO.setup("P9_11", GPIO.IN) 26 | GPIO.add_event_detect("P9_11", GPIO.RISING, callback=mpuC.action) 27 | 28 | try: 29 | while True: 30 | time.sleep(1) 31 | except KeyboardInterrupt: 32 | GPIO.cleanup() 33 | GPIO.cleanup() 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | MPU6050 Python I2C Class 4 | 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 | -------------------------------------------------------------------------------- /bspm_P9_11_27-00A0.dts: -------------------------------------------------------------------------------- 1 | /* 2 | * Generated using http://kilobaser.com/blog/2014-07-28-beaglebone-black-devicetreeoverlay-generator#dtogenerator 3 | * To compile: 4 | * dtc -O dtb -o /lib/firmware/bspm_P9_11_27-00A0.dtbo -b 0 -@ /lib/firmware/bspm_P9_11_27-00A0.dts 5 | * To use: 6 | * echo bspm_P9_11_27 > /sys/devices/bone_capemgr.?/slots 7 | * echo 30 > /sys/class/gpio/export 8 | * Associated directory: 9 | * ls /sys/class/gpio/gpio30/ 10 | * The current value of the pin can be found in the file: 11 | * /sys/class/gpio/gpio30/value 12 | */ 13 | 14 | /dts-v1/; 15 | /plugin/; 16 | 17 | /{ 18 | compatible = "ti,beaglebone", "ti,beaglebone-black"; 19 | part_number = "BS_PINMODE_P9_11_0x27"; 20 | 21 | exclusive-use = 22 | "P9.11", 23 | "gpio0_30"; 24 | 25 | fragment@0 { 26 | target = <&am33xx_pinmux>; 27 | __overlay__ { 28 | bs_pinmode_P9_11_0x27: pinmux_bs_pinmode_P9_11_0x27 { 29 | pinctrl-single,pins = <0x070 0x27>; 30 | }; 31 | }; 32 | }; 33 | 34 | fragment@1 { 35 | target = <&ocp>; 36 | __overlay__ { 37 | bs_pinmode_P9_11_0x27_pinmux { 38 | compatible = "gpio-keys"; 39 | status = "okay"; 40 | pinctrl-names = "default"; 41 | debounce_interval = <50>; 42 | linux,code = <1>; 43 | pinctrl-0 = <&bs_pinmode_P9_11_0x27>; 44 | gpio-key,wakeup; 45 | }; 46 | }; 47 | }; 48 | }; 49 | -------------------------------------------------------------------------------- /testMPU.py: -------------------------------------------------------------------------------- 1 | __author__ = 'Geir' 2 | 3 | from MPU6050 import MPU6050 4 | from time import clock 5 | 6 | mpu = mpu = MPU6050(1, 0x68, -5489, -1441, 1305, -2, -72, -5, True) 7 | 8 | mpu.dmp_initialize() 9 | mpu.set_DMP_enabled(True) 10 | mpu_int_status = mpu.get_int_status() 11 | print(hex(mpu_int_status)) 12 | while False: 13 | print(mpu.get_acceleration()) 14 | print(mpu.get_rotation()) 15 | 16 | packet_size = mpu.DMP_get_FIFO_packet_size() 17 | print(packet_size) 18 | FIFO_count = mpu.get_FIFO_count() 19 | print(FIFO_count) 20 | 21 | count = 0 22 | FIFO_buffer = [0]*64 23 | overflow = 0 24 | no_overflow = 0 25 | crazy_high_number = 0 26 | start_time = clock() 27 | # FIFO_list = list() 28 | FIFO_count_list = list() 29 | while count < 10000: 30 | FIFO_count = mpu.get_FIFO_count() 31 | # FIFO_list.append(FIFO_count) 32 | mpu_int_status = mpu.get_int_status() 33 | FIFO_count_list.append(mpu_int_status) 34 | while FIFO_count < packet_size: 35 | FIFO_count = mpu.get_FIFO_count() 36 | count += 1 37 | if FIFO_count == 1024: 38 | mpu.reset_FIFO() 39 | overflow += 1 40 | print('count: ' + str(count) + ' overflow: ' + str(overflow)) 41 | else: 42 | no_overflow += 1 43 | FIFO_buffer = mpu.get_FIFO_bytes(FIFO_buffer, packet_size) 44 | # print(FIFO_buffer) 45 | accel = mpu.DMP_get_acceleration_int16(FIFO_buffer) 46 | # print(str(accel.x) + " " + str(accel.y) + " " + str(accel.z)) 47 | if (accel.x > 12000) or (accel.x < -12000) or (accel.y > 12000) or \ 48 | (accel.y < -12000) or (accel.z > 12000) or (accel.z < -12000): 49 | crazy_high += 1 50 | 51 | end_time = clock() 52 | delta_time = end_time - start_time 53 | FIFO_sum = 0 54 | FIFO_max = 0 55 | FIFO_min = 1025 56 | for n in FIFO_count_list: 57 | FIFO_sum += n 58 | if n < FIFO_min: 59 | FIFO_min = n 60 | if n > FIFO_max: 61 | FIFO_max = n 62 | FIFO_average = FIFO_sum / len(FIFO_count_list) 63 | print('FIFO_average: ' + str(FIFO_average)) 64 | print('FIFO_min: ' + str(FIFO_min)) 65 | print('FIFO_max: ' + str(FIFO_max)) 66 | print('overflows: ' + str(overflow)) 67 | print('no_overflows: ' + str(no_overflow)) 68 | print('delta time = ' + str(delta_time)) 69 | # print(FIFO_list) 70 | -------------------------------------------------------------------------------- /MPU6050_example.py: -------------------------------------------------------------------------------- 1 | __author__ = 'Geir Istad' 2 | """ 3 | MPU6050 Python I2C Class - MPU6050 example usage 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 | from MPU6050 import MPU6050 26 | 27 | i2c_bus = 1 28 | device_address = 0x68 29 | # The offsets are different for each device and should be changed 30 | # accordingly using a calibration procedure 31 | x_accel_offset = -5489 32 | y_accel_offset = -1441 33 | z_accel_offset = 1305 34 | x_gyro_offset = -2 35 | y_gyro_offset = -72 36 | z_gyro_offset = -5 37 | enable_debug_output = True 38 | 39 | mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset, 40 | z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset, 41 | enable_debug_output) 42 | 43 | mpu.dmp_initialize() 44 | mpu.set_DMP_enabled(True) 45 | mpu_int_status = mpu.get_int_status() 46 | print(hex(mpu_int_status)) 47 | 48 | packet_size = mpu.DMP_get_FIFO_packet_size() 49 | print(packet_size) 50 | FIFO_count = mpu.get_FIFO_count() 51 | print(FIFO_count) 52 | 53 | count = 0 54 | FIFO_buffer = [0]*64 55 | 56 | FIFO_count_list = list() 57 | while count < 10000: 58 | FIFO_count = mpu.get_FIFO_count() 59 | mpu_int_status = mpu.get_int_status() 60 | 61 | # If overflow is detected by status or fifo count we want to reset 62 | if (FIFO_count == 1024) or (mpu_int_status & 0x10): 63 | mpu.reset_FIFO() 64 | print('overflow!') 65 | # Check if fifo data is ready 66 | elif (mpu_int_status & 0x02): 67 | # Wait until packet_size number of bytes are ready for reading, default 68 | # is 42 bytes 69 | while FIFO_count < packet_size: 70 | FIFO_count = mpu.get_FIFO_count() 71 | FIFO_buffer = mpu.get_FIFO_bytes(packet_size) 72 | accel = mpu.DMP_get_acceleration_int16(FIFO_buffer) 73 | quat = mpu.DMP_get_quaternion_int16(FIFO_buffer) 74 | grav = mpu.DMP_get_gravity(quat) 75 | roll_pitch_yaw = mpu.DMP_get_euler_roll_pitch_yaw(quat, grav) 76 | if count % 100 == 0: 77 | print('roll: ' + str(roll_pitch_yaw.x)) 78 | print('pitch: ' + str(roll_pitch_yaw.y)) 79 | print('yaw: ' + str(roll_pitch_yaw.z)) 80 | count += 1 81 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /MPU6050_cal.py: -------------------------------------------------------------------------------- 1 | from MPU6050 import MPU6050 2 | from SimplePID import SimplePID 3 | 4 | 5 | def avg_from_array(a_array): 6 | sum = 0.0 7 | for index in range(0, len(a_array)): 8 | sum += a_array[index] 9 | 10 | return sum/len(a_array) 11 | 12 | 13 | i2c_bus = 1 14 | device_address = 0x68 15 | # The offsets are different for each device and should be changed 16 | # accordingly using a calibration procedure 17 | x_accel_offset = 0 18 | y_accel_offset = 0 19 | z_accel_offset =0 20 | x_gyro_offset = 0 21 | y_gyro_offset = 0 22 | z_gyro_offset = 0 23 | enable_debug_output = True 24 | 25 | mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset, 26 | z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset, 27 | enable_debug_output) 28 | 29 | kp = 0.03125 30 | ki = 0.25 31 | kd = 0 32 | 33 | pidax = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 34 | piday = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 35 | pidaz = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 36 | pidgx = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 37 | pidgy = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 38 | pidgz = SimplePID(0, -15000, 15000, kp, ki, kd, 100, True) 39 | 40 | accel_reading = mpu.get_acceleration() 41 | 42 | x_accel_reading = accel_reading[0] 43 | y_accel_reading = accel_reading[1] 44 | z_accel_reading = accel_reading[2] 45 | 46 | x_accel_avg = [0]*100 47 | y_accel_avg = [0]*100 48 | z_accel_avg = [0]*100 49 | 50 | x_accel_offset_avg = [0]*100 51 | y_accel_offset_avg = [0]*100 52 | z_accel_offset_avg = [0]*100 53 | 54 | axindex = 0 55 | ayindex = 0 56 | azindex = 0 57 | 58 | gyro_reading = mpu.get_rotation() 59 | 60 | x_gyro_reading = gyro_reading[0] 61 | y_gyro_reading = gyro_reading[1] 62 | z_gyro_reading = gyro_reading[2] 63 | 64 | x_gyro_avg = [0]*100 65 | y_gyro_avg = [0]*100 66 | z_gyro_avg = [0]*100 67 | 68 | x_gyro_offset_avg = [0]*100 69 | y_gyro_offset_avg = [0]*100 70 | z_gyro_offset_avg = [0]*100 71 | 72 | gxindex = 0 73 | gyindex = 0 74 | gzindex = 0 75 | 76 | try: 77 | while True: 78 | accel_reading = mpu.get_acceleration() 79 | x_accel_reading = accel_reading[0] 80 | y_accel_reading = accel_reading[1] 81 | z_accel_reading = accel_reading[2] 82 | 83 | gyro_reading = mpu.get_rotation() 84 | x_gyro_reading = gyro_reading[0] 85 | y_gyro_reading = gyro_reading[1] 86 | z_gyro_reading = gyro_reading[2] 87 | 88 | if pidax.check_time(): 89 | x_accel_offset = pidax.get_output_value(x_accel_reading) 90 | 91 | mpu.set_x_accel_offset(int(x_accel_offset)) 92 | 93 | x_accel_avg[axindex] = x_accel_reading 94 | x_accel_offset_avg[axindex] = x_accel_offset 95 | 96 | axindex += 1 97 | if axindex == len(x_accel_avg): 98 | axindex = 0 99 | print('x_avg_read: ' + 100 | str(avg_from_array(x_accel_avg)) + 101 | ' x_avg_offset: ' + 102 | str(avg_from_array(x_accel_offset_avg))) 103 | print('y_avg_read: ' + 104 | str(avg_from_array(y_accel_avg)) + 105 | ' y_avg_offset: ' + 106 | str(avg_from_array(y_accel_offset_avg))) 107 | print('z_avg_read: ' + 108 | str(avg_from_array(z_accel_avg)) + 109 | ' z_avg_offset: ' + 110 | str(avg_from_array(z_accel_offset_avg))) 111 | 112 | if piday.check_time(): 113 | y_accel_offset = piday.get_output_value(y_accel_reading) 114 | 115 | mpu.set_y_accel_offset(int(y_accel_offset)) 116 | 117 | y_accel_avg[ayindex] = y_accel_reading 118 | y_accel_offset_avg[ayindex] = y_accel_offset 119 | 120 | ayindex += 1 121 | if ayindex == len(y_accel_avg): 122 | ayindex = 0 123 | 124 | if pidaz.check_time(): 125 | z_accel_offset = pidaz.get_output_value(z_accel_reading) 126 | 127 | mpu.set_z_accel_offset(int(z_accel_offset)) 128 | 129 | z_accel_avg[azindex] = z_accel_reading 130 | z_accel_offset_avg[azindex] = z_accel_offset 131 | 132 | azindex += 1 133 | if azindex == len(z_accel_avg): 134 | azindex = 0 135 | 136 | # Gyro calibration 137 | if pidgx.check_time(): 138 | x_gyro_offset = pidgx.get_output_value(x_gyro_reading) 139 | 140 | mpu.set_x_gyro_offset(int(x_gyro_offset)) 141 | 142 | x_gyro_avg[gxindex] = x_gyro_reading 143 | x_gyro_offset_avg[gxindex] = x_gyro_offset 144 | 145 | gxindex += 1 146 | if gxindex == len(x_gyro_avg): 147 | gxindex = 0 148 | print('x_avg_read: ' + 149 | str(avg_from_array(x_gyro_avg)) + 150 | ' x_avg_offset: ' + 151 | str(avg_from_array(x_gyro_offset_avg))) 152 | print('y_avg_read: ' + 153 | str(avg_from_array(y_gyro_avg)) + 154 | ' y_avg_offset: ' + 155 | str(avg_from_array(y_gyro_offset_avg))) 156 | print('z_avg_read: ' + 157 | str(avg_from_array(z_gyro_avg)) + 158 | ' z_avg_offset: ' + 159 | str(avg_from_array(z_gyro_offset_avg))) 160 | 161 | if pidgy.check_time(): 162 | y_gyro_offset = pidgy.get_output_value(y_gyro_reading) 163 | 164 | mpu.set_y_gyro_offset(int(y_gyro_offset)) 165 | 166 | y_gyro_avg[gyindex] = y_gyro_reading 167 | y_gyro_offset_avg[gyindex] = y_gyro_offset 168 | 169 | gyindex += 1 170 | if gyindex == len(y_gyro_avg): 171 | gyindex = 0 172 | 173 | if pidgz.check_time(): 174 | z_gyro_offset = pidgz.get_output_value(z_gyro_reading) 175 | 176 | mpu.set_z_gyro_offset(int(z_gyro_offset)) 177 | 178 | z_gyro_avg[gzindex] = z_gyro_reading 179 | z_gyro_offset_avg[gzindex] = z_gyro_offset 180 | 181 | gzindex += 1 182 | if gzindex == len(z_gyro_avg): 183 | gzindex = 0 184 | 185 | except KeyboardInterrupt: 186 | pass 187 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /MPU6050.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 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 | 59 | class MPU6050: 60 | __buffer = [0] * 14 61 | __debug = False 62 | __DMP_packet_size = 0 63 | __dev_id = 0 64 | __bus = None 65 | 66 | def __init__(self, a_bus=1, a_address=C.MPU6050_DEFAULT_ADDRESS, 67 | a_xAOff=None, a_yAOff=None, a_zAOff=None, a_xGOff=None, 68 | a_yGOff=None, a_zGOff=None, a_debug=False): 69 | self.__dev_id = a_address 70 | # Connect to num 1 SMBus 71 | self.__bus = smbus.SMBus(a_bus) 72 | # Set clock source to gyro 73 | self.set_clock_source(C.MPU6050_CLOCK_PLL_XGYRO) 74 | # Set accelerometer range 75 | self.set_full_scale_accel_range(C.MPU6050_ACCEL_FS_2) 76 | # Set gyro range 77 | self.set_full_scale_gyro_range(C.MPU6050_GYRO_FS_250) 78 | # Take the MPU out of time.sleep mode 79 | self.wake_up() 80 | # Set offsets 81 | if a_xAOff: 82 | self.set_x_accel_offset(a_xAOff) 83 | if a_yAOff: 84 | self.set_y_accel_offset(a_yAOff) 85 | if a_zAOff: 86 | self.set_z_accel_offset(a_zAOff) 87 | if a_xGOff: 88 | self.set_x_gyro_offset(a_xGOff) 89 | if a_yGOff: 90 | self.set_y_gyro_offset(a_yGOff) 91 | if a_zGOff: 92 | self.set_z_gyro_offset(a_zGOff) 93 | self.__debug = a_debug 94 | 95 | # Core bit and byte operations 96 | def read_bit(self, a_reg_add, a_bit_position): 97 | return self.read_bits(a_reg_add, a_bit_position, 1) 98 | 99 | def write_bit(self, a_reg_add, a_bit_num, a_bit): 100 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add) 101 | if a_bit: 102 | byte |= 1 << a_bit_num 103 | else: 104 | byte &= ~(1 << a_bit_num) 105 | self.__bus.write_byte_data( 106 | self.__dev_id, a_reg_add, ctypes.c_int8(byte).value) 107 | 108 | def read_bits(self, a_reg_add, a_bit_start, a_length): 109 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add) 110 | mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1) 111 | byte &= mask 112 | byte >>= a_bit_start - a_length + 1 113 | return byte 114 | 115 | def write_bits(self, a_reg_add, a_bit_start, a_length, a_data): 116 | byte = self.__bus.read_byte_data(self.__dev_id, a_reg_add) 117 | mask = ((1 << a_length) - 1) << (a_bit_start - a_length + 1) 118 | # Get data in position and zero all non-important bits in data 119 | a_data <<= a_bit_start - a_length + 1 120 | a_data &= mask 121 | # Clear all important bits in read byte and combine with data 122 | byte &= ~mask 123 | byte = byte | a_data 124 | # Write the data to the I2C device 125 | self.__bus.write_byte_data( 126 | self.__dev_id, a_reg_add, ctypes.c_int8(byte).value) 127 | 128 | def read_memory_byte(self): 129 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_MEM_R_W) 130 | 131 | def read_bytes(self, a_data_list, a_address, a_length): 132 | if a_length > len(a_data_list): 133 | print('read_bytes, length of passed list too short') 134 | return a_data_list 135 | # Attempt to use the built in read bytes function in the adafruit lib 136 | # a_data_list = self.__bus.read_i2c_block_data(self.__dev_id, a_address, 137 | # a_length) 138 | # Attempt to bypass adafruit lib 139 | #a_data_list = self.__mpu.bus.read_i2c_block_data(0x68, a_address, a_length) 140 | #print('data' + str(a_data_list)) 141 | for x in xrange(0, a_length): 142 | a_data_list[x] = self.__bus.read_byte_data(self.__dev_id, 143 | a_address + x) 144 | return a_data_list 145 | 146 | def write_memory_block(self, a_data_list, a_data_size, a_bank, a_address, 147 | a_verify): 148 | success = True 149 | self.set_memory_bank(a_bank) 150 | self.set_memory_start_address(a_address) 151 | 152 | # For each a_data_item we want to write it to the board to a certain 153 | # memory bank and address 154 | for i in range(0, a_data_size): 155 | # Write each data to memory 156 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_MEM_R_W, 157 | a_data_list[i]) 158 | 159 | if a_verify: 160 | self.set_memory_bank(a_bank) 161 | self.set_memory_start_address(a_address) 162 | verify_data = self.__bus.read_byte_data(self.__dev_id, 163 | C.MPU6050_RA_MEM_R_W) 164 | if verify_data != a_data_list[i]: 165 | success = False 166 | 167 | # If we've filled the bank, change the memory bank 168 | if a_address == 255: 169 | a_address = 0 170 | a_bank += 1 171 | self.set_memory_bank(a_bank) 172 | else: 173 | a_address += 1 174 | 175 | # Either way update the memory address 176 | self.set_memory_start_address(a_address) 177 | 178 | return success 179 | 180 | def wake_up(self): 181 | self.write_bit( 182 | C.MPU6050_RA_PWR_MGMT_1, C.MPU6050_PWR1_SLEEP_BIT, 0) 183 | 184 | def set_clock_source(self, a_source): 185 | self.write_bits(C.MPU6050_RA_PWR_MGMT_1, C.MPU6050_PWR1_CLKSEL_BIT, 186 | C.MPU6050_PWR1_CLKSEL_LENGTH, a_source) 187 | 188 | def set_full_scale_gyro_range(self, a_data): 189 | self.write_bits(C.MPU6050_RA_GYRO_CONFIG, 190 | C.MPU6050_GCONFIG_FS_SEL_BIT, 191 | C.MPU6050_GCONFIG_FS_SEL_LENGTH, a_data) 192 | 193 | def set_full_scale_accel_range(self, a_data): 194 | self.write_bits(C.MPU6050_RA_ACCEL_CONFIG, 195 | C.MPU6050_ACONFIG_AFS_SEL_BIT, 196 | C.MPU6050_ACONFIG_AFS_SEL_LENGTH, a_data) 197 | 198 | def reset(self): 199 | self.write_bit(C.MPU6050_RA_PWR_MGMT_1, 200 | C.MPU6050_PWR1_DEVICE_RESET_BIT, 1) 201 | 202 | def set_sleep_enabled(self, a_enabled): 203 | set_bit = 0 204 | if a_enabled: 205 | set_bit = 1 206 | self.write_bit(C.MPU6050_RA_PWR_MGMT_1, 207 | C.MPU6050_PWR1_SLEEP_BIT, set_bit) 208 | 209 | def set_memory_bank(self, a_bank, a_prefetch_enabled=False, 210 | a_user_bank=False): 211 | a_bank &= 0x1F 212 | if a_user_bank: 213 | a_bank |= 0x20 214 | if a_prefetch_enabled: 215 | a_bank |= 0x20 216 | self.__bus.write_byte_data( 217 | self.__dev_id, C.MPU6050_RA_BANK_SEL, a_bank) 218 | 219 | def set_memory_start_address(self, a_address): 220 | self.__bus.write_byte_data( 221 | self.__dev_id, C.MPU6050_RA_MEM_START_ADDR, a_address) 222 | 223 | def get_x_gyro_offset_TC(self): 224 | return self.read_bits(C.MPU6050_RA_XG_OFFS_TC, 225 | C.MPU6050_TC_OFFSET_BIT, 226 | C.MPU6050_TC_OFFSET_LENGTH) 227 | 228 | def set_x_gyro_offset_TC(self, a_offset): 229 | self.write_bits(C.MPU6050_RA_XG_OFFS_TC, 230 | C.MPU6050_TC_OFFSET_BIT, 231 | C.MPU6050_TC_OFFSET_LENGTH, a_offset) 232 | 233 | def get_y_gyro_offset_TC(self): 234 | return self.read_bits(C.MPU6050_RA_YG_OFFS_TC, 235 | C.MPU6050_TC_OFFSET_BIT, 236 | C.MPU6050_TC_OFFSET_LENGTH) 237 | 238 | def set_y_gyro_offset_TC(self, a_offset): 239 | self.write_bits(C.MPU6050_RA_YG_OFFS_TC, 240 | C.MPU6050_TC_OFFSET_BIT, 241 | C.MPU6050_TC_OFFSET_LENGTH, a_offset) 242 | 243 | def get_z_gyro_offset_TC(self): 244 | return self.read_bits(C.MPU6050_RA_ZG_OFFS_TC, 245 | C.MPU6050_TC_OFFSET_BIT, 246 | C.MPU6050_TC_OFFSET_LENGTH) 247 | 248 | def set_z_gyro_offset_TC(self, a_offset): 249 | self.write_bits(C.MPU6050_RA_ZG_OFFS_TC, 250 | C.MPU6050_TC_OFFSET_BIT, 251 | C.MPU6050_TC_OFFSET_LENGTH, a_offset) 252 | 253 | def set_slave_address(self, a_num, a_address): 254 | self.__bus.write_byte_data( 255 | self.__dev_id, C.MPU6050_RA_I2C_SLV0_ADDR + a_num * 3, a_address) 256 | 257 | def set_I2C_master_mode_enabled(self, a_enabled): 258 | bit = 0 259 | if a_enabled: 260 | bit = 1 261 | self.write_bit(C.MPU6050_RA_USER_CTRL, 262 | C.MPU6050_USERCTRL_I2C_MST_EN_BIT, bit) 263 | 264 | def reset_I2C_master(self): 265 | self.write_bit(C.MPU6050_RA_USER_CTRL, 266 | C.MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1) 267 | 268 | def write_prog_memory_block(self, a_data_list, a_data_size, a_bank=0, 269 | a_address=0, a_verify=True): 270 | return self.write_memory_block(a_data_list, a_data_size, a_bank, 271 | a_address, a_verify) 272 | 273 | def write_DMP_configuration_set(self, a_data_list, a_data_size): 274 | index = 0 275 | while index < a_data_size: 276 | bank = a_data_list[index] 277 | offset = a_data_list[index + 1] 278 | length = a_data_list[index + 2] 279 | index += 3 280 | success = False 281 | 282 | # Normal case 283 | if length > 0: 284 | data_selection = list() 285 | for subindex in range(0, length): 286 | data_selection.append(a_data_list[index + subindex]) 287 | success = self.write_memory_block(data_selection, length, bank, 288 | offset, True) 289 | index += length 290 | # Special undocumented case 291 | else: 292 | special = a_data_list[index] 293 | index += 1 294 | if special == 0x01: 295 | # TODO Figure out if write8 can return True/False 296 | success = self.__bus.write_byte_data( 297 | self.__dev_id, C.MPU6050_RA_INT_ENABLE, 0x32) 298 | 299 | if success == False: 300 | # TODO implement error messagemajigger 301 | return False 302 | pass 303 | return True 304 | 305 | def write_prog_dmp_configuration(self, a_data_list, a_data_size): 306 | return self.write_DMP_configuration_set(a_data_list, a_data_size) 307 | 308 | def set_int_enable(self, a_enabled): 309 | self.__bus.write_byte_data( 310 | self.__dev_id, C.MPU6050_RA_INT_ENABLE, a_enabled) 311 | 312 | def set_rate(self, a_rate): 313 | self.__bus.write_byte_data( 314 | self.__dev_id, C.MPU6050_RA_SMPLRT_DIV, a_rate) 315 | 316 | def set_external_frame_sync(self, a_sync): 317 | self.write_bits(C.MPU6050_RA_CONFIG, 318 | C.MPU6050_CFG_EXT_SYNC_SET_BIT, 319 | C.MPU6050_CFG_EXT_SYNC_SET_LENGTH, a_sync) 320 | 321 | def set_DLF_mode(self, a_mode): 322 | self.write_bits(C.MPU6050_RA_CONFIG, C.MPU6050_CFG_DLPF_CFG_BIT, 323 | C.MPU6050_CFG_DLPF_CFG_LENGTH, a_mode) 324 | 325 | def get_DMP_config_1(self): 326 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_DMP_CFG_1) 327 | 328 | def set_DMP_config_1(self, a_config): 329 | self.__bus.write_byte_data( 330 | self.__dev_id, C.MPU6050_RA_DMP_CFG_1, a_config) 331 | 332 | def get_DMP_config_2(self): 333 | return self.__bus.read_byte_data(self.__dev_id, C.MPU6050_RA_DMP_CFG_2) 334 | 335 | def set_DMP_config_2(self, a_config): 336 | self.__bus.write_byte_data( 337 | self.__dev_id, C.MPU6050_RA_DMP_CFG_2, a_config) 338 | 339 | def set_OTP_bank_valid(self, a_enabled): 340 | bit = 0 341 | if a_enabled: 342 | bit = 1 343 | self.write_bit(C.MPU6050_RA_XG_OFFS_TC, 344 | C.MPU6050_TC_OTP_BNK_VLD_BIT, bit) 345 | 346 | def get_OTP_bank_valid(self): 347 | return self.read_bit(C.MPU6050_RA_XG_OFFS_TC, 348 | C.MPU6050_TC_OTP_BNK_VLD_BIT) 349 | 350 | def set_motion_detection_threshold(self, a_threshold): 351 | self.__bus.write_byte_data( 352 | self.__dev_id, C.MPU6050_RA_MOT_THR, a_threshold) 353 | 354 | def set_zero_motion_detection_threshold(self, a_threshold): 355 | self.__bus.write_byte_data( 356 | self.__dev_id, C.MPU6050_RA_ZRMOT_THR, a_threshold) 357 | 358 | def set_motion_detection_duration(self, a_duration): 359 | self.__bus.write_byte_data( 360 | self.__dev_id, C.MPU6050_RA_MOT_DUR, a_duration) 361 | 362 | def set_zero_motion_detection_duration(self, a_duration): 363 | self.__bus.write_byte_data( 364 | self.__dev_id, C.MPU6050_RA_ZRMOT_DUR, a_duration) 365 | 366 | def set_FIFO_enabled(self, a_enabled): 367 | bit = 0 368 | if a_enabled: 369 | bit = 1 370 | self.write_bit(C.MPU6050_RA_USER_CTRL, 371 | C.MPU6050_USERCTRL_FIFO_EN_BIT, bit) 372 | 373 | def set_DMP_enabled(self, a_enabled): 374 | bit = 0 375 | if a_enabled: 376 | bit = 1 377 | self.write_bit(C.MPU6050_RA_USER_CTRL, 378 | C.MPU6050_USERCTRL_DMP_EN_BIT, bit) 379 | 380 | def reset_DMP(self): 381 | self.write_bit(C.MPU6050_RA_USER_CTRL, 382 | C.MPU6050_USERCTRL_DMP_RESET_BIT, True) 383 | 384 | def dmp_initialize(self): 385 | # Reset the MPU 386 | self.reset() 387 | # time.Sleep a bit while resetting 388 | time.sleep(50 / 1000) 389 | # Disable time.sleep mode 390 | self.set_sleep_enabled(0) 391 | 392 | # get MPU hardware revision 393 | if self.__debug: 394 | print('Selecting user bank 16') 395 | self.set_memory_bank(0x10, True, True) 396 | 397 | if self.__debug: 398 | print('Selecting memory byte 6') 399 | self.set_memory_start_address(0x6) 400 | 401 | if self.__debug: 402 | print('Checking hardware revision') 403 | HW_revision = self.read_memory_byte() 404 | if self.__debug: 405 | print('Revision @ user[16][6] = ' + hex(HW_revision)) 406 | 407 | if self.__debug: 408 | print('Resetting memory bank selection to 0') 409 | self.set_memory_bank(0) 410 | 411 | # check OTP bank valid 412 | # TODO Find out what OTP is 413 | OTP_valid = self.get_OTP_bank_valid() 414 | if self.__debug: 415 | if OTP_valid: 416 | print('OTP bank is valid') 417 | else: 418 | print('OTP bank is invalid') 419 | 420 | # get X/Y/Z gyro offsets 421 | if self.__debug: 422 | print('Reading gyro offet TC values') 423 | x_g_offset_TC = self.get_x_gyro_offset_TC() 424 | y_g_offset_TC = self.get_y_gyro_offset_TC() 425 | z_g_offset_TC = self.get_z_gyro_offset_TC() 426 | if self.__debug: 427 | print("X gyro offset = ", repr(x_g_offset_TC)) 428 | print("Y gyro offset = ", repr(y_g_offset_TC)) 429 | print("Z gyro offset = ", repr(z_g_offset_TC)) 430 | 431 | # setup weird slave stuff (?) 432 | if self.__debug: 433 | print('Setting slave 0 address to 0x7F') 434 | self.set_slave_address(0, 0x7F) 435 | if self.__debug: 436 | print('Disabling I2C Master mode') 437 | self.set_I2C_master_mode_enabled(False) 438 | if self.__debug: 439 | print('Setting slave 0 address to 0x68 (self)') 440 | self.set_slave_address(0, 0x68) 441 | if self.__debug: 442 | print('Resetting I2C Master control') 443 | self.reset_I2C_master() 444 | # Wait a bit for the device to register the changes 445 | time.sleep(20 / 1000) 446 | 447 | # load DMP code into memory banks 448 | if self.__debug: 449 | print('Writing DMP code to MPU memory banks ' + 450 | repr(C.MPU6050_DMP_CODE_SIZE) + ' bytes') 451 | if self.write_prog_memory_block(C.dmpMemory, C.MPU6050_DMP_CODE_SIZE): 452 | # TODO Check if we've actually verified this 453 | if self.__debug: 454 | print('Success! DMP code written and verified') 455 | 456 | # Write DMP configuration 457 | if self.__debug: 458 | print('Writing DMP configuration to MPU memory banks ' + 459 | repr(C.MPU6050_DMP_CONFIG_SIZE) + ' bytes in config') 460 | if self.write_prog_dmp_configuration(C.dmpConfig, 461 | C.MPU6050_DMP_CONFIG_SIZE): 462 | if self.__debug: 463 | print('Success! DMP configuration written and verified.') 464 | print('Setting clock source to Z gyro') 465 | self.set_clock_source(C.MPU6050_CLOCK_PLL_ZGYRO) 466 | 467 | if self.__debug: 468 | print('Setting DMP and FIFO_OFLOW interrupts enabled') 469 | self.set_int_enable(0x12) 470 | 471 | if self.__debug: 472 | print('Setting sample rate to 200Hz') 473 | self.set_rate(4) 474 | 475 | if self.__debug: 476 | print('Setting external frame sync to TEMP_OUT_L[0]') 477 | self.set_external_frame_sync(C.MPU6050_EXT_SYNC_TEMP_OUT_L) 478 | 479 | if self.__debug: 480 | print('Setting DLPF bandwidth to 42Hz') 481 | self.set_DLF_mode(C.MPU6050_DLPF_BW_42) 482 | 483 | if self.__debug: 484 | print('Setting gyro sensitivity to +/- 2000 deg/sec') 485 | self.set_full_scale_gyro_range(C.MPU6050_GYRO_FS_2000) 486 | 487 | if self.__debug: 488 | print('Setting DMP configuration bytes (function unknown)') 489 | self.set_DMP_config_1(0x03) 490 | self.set_DMP_config_2(0x00) 491 | 492 | if self.__debug: 493 | print('Clearing OTP Bank flag') 494 | self.set_OTP_bank_valid(False) 495 | 496 | if self.__debug: 497 | print('Setting X/Y/Z gyro offset TCs to previous values') 498 | self.set_x_gyro_offset_TC(x_g_offset_TC) 499 | self.set_y_gyro_offset_TC(y_g_offset_TC) 500 | self.set_z_gyro_offset_TC(z_g_offset_TC) 501 | 502 | # Uncomment this to zero offsets when dmp_initialize is called 503 | # if self.__debug: 504 | # print('Setting X/Y/Z gyro user offsets to zero') 505 | # self.set_x_gyro_offset(0) 506 | # self.set_y_gyro_offset(0) 507 | # self.set_z_gyro_offset(0) 508 | 509 | if self.__debug: 510 | print('Writing final memory update 1/7 (function unknown)') 511 | pos = 0 512 | j = 0 513 | dmp_update = [0] * 16 514 | while (j < 4) or (j < dmp_update[2] + 3): 515 | dmp_update[j] = C.dmpUpdates[pos] 516 | pos += 1 517 | j += 1 518 | # Write as block from pos 3 519 | self.write_memory_block(dmp_update[3:], dmp_update[2], 520 | dmp_update[0], dmp_update[1], True) 521 | 522 | if self.__debug: 523 | print('Writing final memory update 2/7 (function unknown)') 524 | j = 0 525 | while (j < 4) or (j < dmp_update[2] + 3): 526 | dmp_update[j] = C.dmpUpdates[pos] 527 | pos += 1 528 | j += 1 529 | # Write as block from pos 3 530 | self.write_memory_block(dmp_update[3:], dmp_update[2], 531 | dmp_update[0], dmp_update[1], True) 532 | 533 | if self.__debug: 534 | print('Resetting FIFO') 535 | self.reset_FIFO() 536 | 537 | if self.__debug: 538 | print('Reading FIFO count') 539 | FIFO_count = self.get_FIFO_count() 540 | 541 | if self.__debug: 542 | print('FIFO count: ' + repr(FIFO_count)) 543 | 544 | if self.__debug: 545 | print('Getting FIFO buffer') 546 | FIFO_buffer = [0] * 128 547 | FIFO_buffer = self.get_FIFO_bytes(FIFO_count) 548 | 549 | if self.__debug: 550 | print('Setting motion detection threshold to 2') 551 | self.set_motion_detection_threshold(2) 552 | 553 | if self.__debug: 554 | print('Setting zero-motion detection threshold to 156') 555 | self.set_zero_motion_detection_threshold(156) 556 | 557 | if self.__debug: 558 | print('Setting motion detection duration to 80') 559 | self.set_motion_detection_duration(80) 560 | 561 | if self.__debug: 562 | print('Setting zero-motion detection duration to 0') 563 | self.set_zero_motion_detection_duration(0) 564 | 565 | if self.__debug: 566 | print('Resetting FIFO') 567 | self.reset_FIFO() 568 | 569 | if self.__debug: 570 | print('Enabling FIFO') 571 | self.set_FIFO_enabled(True) 572 | 573 | if self.__debug: 574 | print('Enabling DMP') 575 | self.set_DMP_enabled(True) 576 | 577 | if self.__debug: 578 | print('Resetting DMP') 579 | self.reset_DMP() 580 | 581 | if self.__debug: 582 | print('Writing final memory update 3/7 (function unknown)') 583 | j = 0 584 | while (j < 4) or (j < dmp_update[2] + 3): 585 | dmp_update[j] = C.dmpUpdates[pos] 586 | pos += 1 587 | j += 1 588 | # Write as block from pos 3 589 | self.write_memory_block(dmp_update[3:], dmp_update[2], 590 | dmp_update[0], dmp_update[1], True) 591 | 592 | if self.__debug: 593 | print('Writing final memory update 4/7 (function unknown)') 594 | j = 0 595 | while (j < 4) or (j < dmp_update[2] + 3): 596 | dmp_update[j] = C.dmpUpdates[pos] 597 | pos += 1 598 | j += 1 599 | # Write as block from pos 3 600 | self.write_memory_block(dmp_update[3:], dmp_update[2], 601 | dmp_update[0], dmp_update[1], True) 602 | 603 | if self.__debug: 604 | print('Writing final memory update 5/7 (function unknown)') 605 | j = 0 606 | while (j < 4) or (j < dmp_update[2] + 3): 607 | dmp_update[j] = C.dmpUpdates[pos] 608 | pos += 1 609 | j += 1 610 | # Write as block from pos 3 611 | self.write_memory_block(dmp_update[3:], dmp_update[2], 612 | dmp_update[0], dmp_update[1], True) 613 | 614 | if self.__debug: 615 | print('Waiting for FIFO count > 2') 616 | FIFO_count = self.get_FIFO_count() 617 | while FIFO_count < 3: 618 | FIFO_count = self.get_FIFO_count() 619 | 620 | if self.__debug: 621 | print('Current FIFO count = ' + repr(FIFO_count)) 622 | print('Reading FIFO data') 623 | FIFO_buffer = self.get_FIFO_bytes(FIFO_count) 624 | 625 | if self.__debug: 626 | print('Reading interrupt status') 627 | MPU_int_status = self.get_int_status() 628 | 629 | if self.__debug: 630 | print('Current interrupt status = ' + hex(MPU_int_status)) 631 | print('Writing final memory update 6/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 count') 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 7/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('DMP is good to go! Finally.') 670 | print('Disabling DMP (you turn it on later)') 671 | self.set_DMP_enabled(False) 672 | 673 | if self.__debug: 674 | print('Setting up internal 42 byte DMP packet buffer') 675 | self.__DMP_packet_size = 42 676 | 677 | if self.__debug: 678 | print( 679 | 'Resetting FIFO and clearing INT status one last time') 680 | self.reset_FIFO() 681 | self.get_int_status() 682 | 683 | else: 684 | if self.__debug: 685 | print('Configuration block loading failed') 686 | return 2 687 | 688 | else: 689 | if self.__debug: 690 | print('Main binary block loading failed') 691 | return 1 692 | 693 | if self.__debug: 694 | print('DMP initialization was successful') 695 | return 0 696 | 697 | # Acceleration and gyro offset setters and getters 698 | def set_x_accel_offset(self, a_offset): 699 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XA_OFFS_H, 700 | ctypes.c_int8(a_offset >> 8).value) 701 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XA_OFFS_L_TC, 702 | ctypes.c_int8(a_offset).value) 703 | 704 | def set_y_accel_offset(self, a_offset): 705 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YA_OFFS_H, 706 | ctypes.c_int8(a_offset >> 8).value) 707 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YA_OFFS_L_TC, 708 | ctypes.c_int8(a_offset).value) 709 | 710 | def set_z_accel_offset(self, a_offset): 711 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZA_OFFS_H, 712 | ctypes.c_int8(a_offset >> 8).value) 713 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZA_OFFS_L_TC, 714 | ctypes.c_int8(a_offset).value) 715 | 716 | def set_x_gyro_offset(self, a_offset): 717 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XG_OFFS_USRH, 718 | ctypes.c_int8(a_offset >> 8).value) 719 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_XG_OFFS_USRL, 720 | ctypes.c_int8(a_offset).value) 721 | 722 | def set_y_gyro_offset(self, a_offset): 723 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YG_OFFS_USRH, 724 | ctypes.c_int8(a_offset >> 8).value) 725 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_YG_OFFS_USRL, 726 | ctypes.c_int8(a_offset).value) 727 | 728 | def set_z_gyro_offset(self, a_offset): 729 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZG_OFFS_USRH, 730 | ctypes.c_int8(a_offset >> 8).value) 731 | self.__bus.write_byte_data(self.__dev_id, C.MPU6050_RA_ZG_OFFS_USRL, 732 | ctypes.c_int8(a_offset).value) 733 | 734 | # Main interfacing functions to get raw data from MPU 735 | def get_acceleration(self): 736 | raw_data = self.__bus.read_i2c_block_data(self.__dev_id, 737 | C.MPU6050_RA_ACCEL_XOUT_H, 6) 738 | accel = [0] * 3 739 | accel[0] = ctypes.c_int16(raw_data[0] << 8 | raw_data[1]).value 740 | accel[1] = ctypes.c_int16(raw_data[2] << 8 | raw_data[3]).value 741 | accel[2] = ctypes.c_int16(raw_data[4] << 8 | raw_data[5]).value 742 | return accel 743 | 744 | def get_rotation(self): 745 | raw_data = self.__bus.read_i2c_block_data(self.__dev_id, 746 | C.MPU6050_RA_GYRO_XOUT_H, 6) 747 | gyro = [0] * 3 748 | gyro[0] = ctypes.c_int16(raw_data[0] << 8 | raw_data[1]).value 749 | gyro[1] = ctypes.c_int16(raw_data[2] << 8 | raw_data[3]).value 750 | gyro[2] = ctypes.c_int16(raw_data[4] << 8 | raw_data[5]).value 751 | return gyro 752 | 753 | # Interfacing functions to get data from FIFO buffer 754 | def DMP_get_FIFO_packet_size(self): 755 | return self.__DMP_packet_size 756 | 757 | def reset_FIFO(self): 758 | self.write_bit(C.MPU6050_RA_USER_CTRL, 759 | C.MPU6050_USERCTRL_FIFO_RESET_BIT, True) 760 | 761 | def get_FIFO_count(self): 762 | data = [0] * 2 763 | data = self.read_bytes(data, C.MPU6050_RA_FIFO_COUNTH, 2) 764 | return (data[0] << 8) | data[1] 765 | 766 | def get_FIFO_bytes(self, a_FIFO_count): 767 | return_list = list() 768 | for index in range(0, a_FIFO_count): 769 | return_list.append( 770 | self.__bus.read_byte_data(self.__dev_id, 771 | C.MPU6050_RA_FIFO_R_W)) 772 | return return_list 773 | 774 | def get_int_status(self): 775 | return self.__bus.read_byte_data(self.__dev_id, 776 | C.MPU6050_RA_INT_STATUS) 777 | 778 | # Data retrieval from received FIFO buffer 779 | def DMP_get_quaternion_int16(self, a_FIFO_buffer): 780 | w = ctypes.c_int16((a_FIFO_buffer[0] << 8) | a_FIFO_buffer[1]).value 781 | x = ctypes.c_int16((a_FIFO_buffer[4] << 8) | a_FIFO_buffer[5]).value 782 | y = ctypes.c_int16((a_FIFO_buffer[8] << 8) | a_FIFO_buffer[9]).value 783 | z = ctypes.c_int16((a_FIFO_buffer[12] << 8) | a_FIFO_buffer[13]).value 784 | return Q(w, x, y, z) 785 | 786 | def DMP_get_quaternion(self, a_FIFO_buffer): 787 | quat = self.DMP_get_quaternion_int16(a_FIFO_buffer) 788 | w = quat.w / 16384.0 789 | x = quat.x / 16384.0 790 | y = quat.y / 16384.0 791 | z = quat.z / 16384.0 792 | return Q(w, x, y, z) 793 | 794 | def DMP_get_acceleration_int16(self, a_FIFO_buffer): 795 | x = ctypes.c_int16(a_FIFO_buffer[28] << 8 | a_FIFO_buffer[29]).value 796 | y = ctypes.c_int16(a_FIFO_buffer[32] << 8 | a_FIFO_buffer[33]).value 797 | z = ctypes.c_int16(a_FIFO_buffer[36] << 8 | a_FIFO_buffer[37]).value 798 | return V(x, y, z) 799 | 800 | def DMP_get_gravity(self, a_quat): 801 | x = 2.0 * (a_quat.x * a_quat.z - a_quat.w * a_quat.y) 802 | y = 2.0 * (a_quat.w * a_quat.x + a_quat.y * a_quat.z) 803 | z = 1.0 * (a_quat.w * a_quat.w - a_quat.x * a_quat.x - 804 | a_quat.y * a_quat.y + a_quat.z * a_quat.z) 805 | return V(x, y, z) 806 | 807 | def DMP_get_linear_accel_int16(self, a_v_raw, a_grav): 808 | x = ctypes.c_int16(a_v_raw.x - (a_grav.x*8192)).value 809 | y = ctypes.c_int16(a_v_raw.y - (a_grav.y*8192)).value 810 | y = ctypes.c_int16(a_v_raw.y - (a_grav.y*8192)).value 811 | return V(x, y, z) 812 | 813 | def DMP_get_euler(self, a_quat): 814 | psi = math.atan2(2*a_quat.x*a_quat.y - 2*a_quat.w*a_quat.z, 815 | 2*a_quat.w*a_quat.w + 2*a_quat.x*a_quat.x - 1) 816 | theta = -asin(2*a_quat.x*a_quat.z + 2*a_quat.w*a_quat.y) 817 | phi = math.atan2(2*a_quat.y*a_quat.z - 2*a_quat.w*a_quat.x, 818 | 2*a_quat.w*a_quat.w + 2*a_quat.z*a_quat.z - 1) 819 | return V(psi, theta, phi) 820 | 821 | def DMP_get_roll_pitch_yaw(self, a_quat, a_grav_vect): 822 | # roll: (tilt left/right, about X axis) 823 | roll = math.atan(a_grav_vect.y / 824 | math.sqrt(a_grav_vect.x*a_grav_vect.x + 825 | a_grav_vect.z*a_grav_vect.z)) 826 | # pitch: (nose up/down, about Y axis) 827 | pitch = math.atan(a_grav_vect.x / 828 | math.sqrt(a_grav_vect.y*a_grav_vect.y + 829 | a_grav_vect.z*a_grav_vect.z)) 830 | # yaw: (about Z axis) 831 | yaw = math.atan2(2*a_quat.x*a_quat.y - 2*a_quat.w*a_quat.z, 832 | 2*a_quat.w*a_quat.w + 2*a_quat.x*a_quat.x - 1) 833 | return V(roll, pitch, yaw) 834 | 835 | def DMP_get_euler_roll_pitch_yaw(self, a_quat, a_grav_vect): 836 | rad_ypr = self.DMP_get_roll_pitch_yaw(a_quat, a_grav_vect) 837 | roll = rad_ypr.x * (180.0/math.pi) 838 | pitch = rad_ypr.y * (180.0/math.pi) 839 | yaw = rad_ypr.z * (180.0/math.pi) 840 | return V(roll, pitch, yaw) 841 | 842 | def DMP_get_linear_accel(self, a_vector_raw, a_vect_grav): 843 | x = a_vector_raw.x - a_vect_grav.x*8192 844 | y = a_vector_raw.y - a_vect_grav.y*8192 845 | z = a_vector_raw.z - a_vect_grav.z*8192 846 | return V(x, y, z) 847 | 848 | 849 | class MPU6050IRQHandler: 850 | __mpu = MPU6050 851 | __FIFO_buffer = list() 852 | __count = 0 853 | __packet_size = None 854 | __detected_error = False 855 | __logging = False 856 | __log_file = None 857 | __csv_writer = None 858 | __start_time = None 859 | __debug = None 860 | 861 | # def __init__(self, a_i2c_bus, a_device_address, a_x_accel_offset, 862 | # a_y_accel_offset, a_z_accel_offset, a_x_gyro_offset, 863 | # a_y_gyro_offset, a_z_gyro_offset, a_enable_debug_output): 864 | # self.__mpu = MPU6050(a_i2c_bus, a_device_address, a_x_accel_offset, 865 | # a_y_accel_offset, a_z_accel_offset, 866 | # a_x_gyro_offset, a_y_gyro_offset, a_z_gyro_offset, 867 | # a_enable_debug_output) 868 | def __init__(self, a_mpu, a_logging=False, a_log_file='log.csv', 869 | a_debug=False): 870 | self.__mpu = a_mpu 871 | self.__FIFO_buffer = [0]*64 872 | self.__mpu.dmp_initialize() 873 | self.__mpu.set_DMP_enabled(True) 874 | self.__packet_size = self.__mpu.DMP_get_FIFO_packet_size() 875 | mpu_int_status = self.__mpu.get_int_status() 876 | if a_logging: 877 | self.__start_time = time.clock() 878 | self.__logging = True 879 | self.__log_file = open(a_log_file, 'ab') 880 | self.__csv_writer = csv.writer(self.__log_file, delimiter=',', 881 | quotechar='|', 882 | quoting=csv.QUOTE_MINIMAL) 883 | self.__debug = a_debug 884 | 885 | def action(self, channel): 886 | if self.__detected_error: 887 | # Clear FIFO and reset MPU 888 | mpu_int_status = self.__mpu.get_int_status() 889 | self.__mpu.reset_FIFO() 890 | self.__detected_error = False 891 | return 892 | 893 | try: 894 | FIFO_count = self.__mpu.get_FIFO_count() 895 | mpu_int_status = self.__mpu.get_int_status() 896 | except: 897 | self.__detected_error = True 898 | return 899 | 900 | # If overflow is detected by status or fifo count we want to reset 901 | if (FIFO_count == 1024) or (mpu_int_status & 0x10): 902 | try: 903 | self.__mpu.reset_FIFO() 904 | except: 905 | self.__detected_error = True 906 | return 907 | 908 | elif (mpu_int_status & 0x02): 909 | # Wait until packet_size number of bytes are ready for reading, 910 | # default is 42 bytes 911 | while FIFO_count < self.__packet_size: 912 | try: 913 | FIFO_count = self.__mpu.get_FIFO_count() 914 | except: 915 | self.__detected_error = True 916 | return 917 | 918 | while FIFO_count > self.__packet_size: 919 | 920 | try: 921 | self.__FIFO_buffer = \ 922 | self.__mpu.get_FIFO_bytes(self.__packet_size) 923 | except: 924 | self.__detected_error = True 925 | return 926 | accel = \ 927 | self.__mpu.DMP_get_acceleration_int16(self.__FIFO_buffer) 928 | quat = self.__mpu.DMP_get_quaternion_int16(self.__FIFO_buffer) 929 | grav = self.__mpu.DMP_get_gravity(quat) 930 | roll_pitch_yaw = self.__mpu.DMP_get_euler_roll_pitch_yaw(quat, 931 | grav) 932 | if self.__logging: 933 | delta_time = time.clock() - self.__start_time 934 | data_concat = ['%.4f' % delta_time] + \ 935 | [accel.x, accel.y, accel.z] + \ 936 | ['%.3f' % roll_pitch_yaw.x, 937 | '%.3f' % roll_pitch_yaw.y, 938 | '%.3f' % roll_pitch_yaw.z] 939 | self.__csv_writer.writerow(data_concat) 940 | 941 | if (self.__debug) and (self.__count % 100 == 0): 942 | print('roll: ' + str(roll_pitch_yaw.x)) 943 | print('pitch: ' + str(roll_pitch_yaw.y)) 944 | print('yaw: ' + str(roll_pitch_yaw.z)) 945 | self.__count += 1 946 | FIFO_count -= self.__packet_size 947 | --------------------------------------------------------------------------------