quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

MPU6050_6Axis_MotionApps20.h@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:e63996fd7d3e 1 // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
khaledelmadawi 0:e63996fd7d3e 2 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
khaledelmadawi 0:e63996fd7d3e 3 // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
khaledelmadawi 0:e63996fd7d3e 4 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
khaledelmadawi 0:e63996fd7d3e 5 //
khaledelmadawi 0:e63996fd7d3e 6 // Changelog:
khaledelmadawi 0:e63996fd7d3e 7 // ... - ongoing debug release
khaledelmadawi 0:e63996fd7d3e 8
khaledelmadawi 0:e63996fd7d3e 9 /* ============================================
khaledelmadawi 0:e63996fd7d3e 10 I2Cdev device library code is placed under the MIT license
khaledelmadawi 0:e63996fd7d3e 11 Copyright (c) 2012 Jeff Rowberg
khaledelmadawi 0:e63996fd7d3e 12
khaledelmadawi 0:e63996fd7d3e 13 Permission is hereby granted, free of charge, to any person obtaining a copy
khaledelmadawi 0:e63996fd7d3e 14 of this software and associated documentation files (the "Software"), to deal
khaledelmadawi 0:e63996fd7d3e 15 in the Software without restriction, including without limitation the rights
khaledelmadawi 0:e63996fd7d3e 16 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
khaledelmadawi 0:e63996fd7d3e 17 copies of the Software, and to permit persons to whom the Software is
khaledelmadawi 0:e63996fd7d3e 18 furnished to do so, subject to the following conditions:
khaledelmadawi 0:e63996fd7d3e 19
khaledelmadawi 0:e63996fd7d3e 20 The above copyright notice and this permission notice shall be included in
khaledelmadawi 0:e63996fd7d3e 21 all copies or substantial portions of the Software.
khaledelmadawi 0:e63996fd7d3e 22
khaledelmadawi 0:e63996fd7d3e 23 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
khaledelmadawi 0:e63996fd7d3e 24 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
khaledelmadawi 0:e63996fd7d3e 25 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
khaledelmadawi 0:e63996fd7d3e 26 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
khaledelmadawi 0:e63996fd7d3e 27 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
khaledelmadawi 0:e63996fd7d3e 28 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
khaledelmadawi 0:e63996fd7d3e 29 THE SOFTWARE.
khaledelmadawi 0:e63996fd7d3e 30 ===============================================
khaledelmadawi 0:e63996fd7d3e 31 */
khaledelmadawi 0:e63996fd7d3e 32 #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_
khaledelmadawi 0:e63996fd7d3e 33 #define _MPU6050_6AXIS_MOTIONAPPS20_H_
khaledelmadawi 0:e63996fd7d3e 34
khaledelmadawi 0:e63996fd7d3e 35 #include <iostream>
khaledelmadawi 0:e63996fd7d3e 36
khaledelmadawi 0:e63996fd7d3e 37 #include "I2Cdev.h"
khaledelmadawi 0:e63996fd7d3e 38 #include "helper_3dmath.h"
khaledelmadawi 0:e63996fd7d3e 39
khaledelmadawi 0:e63996fd7d3e 40 // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
khaledelmadawi 0:e63996fd7d3e 41 #define MPU6050_INCLUDE_DMP_MOTIONAPPS20
khaledelmadawi 0:e63996fd7d3e 42
khaledelmadawi 0:e63996fd7d3e 43 #include "MPU6050.h"
khaledelmadawi 0:e63996fd7d3e 44
khaledelmadawi 0:e63996fd7d3e 45 // Tom Carpenter's conditional PROGMEM code
khaledelmadawi 0:e63996fd7d3e 46 // http://forum.arduino.cc/index.php?topic=129407.0
khaledelmadawi 0:e63996fd7d3e 47 #ifndef __arm__
khaledelmadawi 0:e63996fd7d3e 48 #include <avr/pgmspace.h>
khaledelmadawi 0:e63996fd7d3e 49 #else
khaledelmadawi 0:e63996fd7d3e 50 // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
khaledelmadawi 0:e63996fd7d3e 51 #ifndef __PGMSPACE_H_
khaledelmadawi 0:e63996fd7d3e 52 #define __PGMSPACE_H_ 1
khaledelmadawi 0:e63996fd7d3e 53 #include <inttypes.h>
khaledelmadawi 0:e63996fd7d3e 54
khaledelmadawi 0:e63996fd7d3e 55 #define PROGMEM
khaledelmadawi 0:e63996fd7d3e 56 #define PGM_P const char *
khaledelmadawi 0:e63996fd7d3e 57 #define PSTR(str) (str)
khaledelmadawi 0:e63996fd7d3e 58 #define F(x) x
khaledelmadawi 0:e63996fd7d3e 59
khaledelmadawi 0:e63996fd7d3e 60 typedef void prog_void;
khaledelmadawi 0:e63996fd7d3e 61 typedef char prog_char;
khaledelmadawi 0:e63996fd7d3e 62 typedef unsigned char prog_uchar;
khaledelmadawi 0:e63996fd7d3e 63 typedef int8_t prog_int8_t;
khaledelmadawi 0:e63996fd7d3e 64 typedef uint8_t prog_uint8_t;
khaledelmadawi 0:e63996fd7d3e 65 typedef int16_t prog_int16_t;
khaledelmadawi 0:e63996fd7d3e 66 typedef uint16_t prog_uint16_t;
khaledelmadawi 0:e63996fd7d3e 67 typedef int32_t prog_int32_t;
khaledelmadawi 0:e63996fd7d3e 68 typedef uint32_t prog_uint32_t;
khaledelmadawi 0:e63996fd7d3e 69
khaledelmadawi 0:e63996fd7d3e 70 #define strcpy_P(dest, src) strcpy((dest), (src))
khaledelmadawi 0:e63996fd7d3e 71 #define strcat_P(dest, src) strcat((dest), (src))
khaledelmadawi 0:e63996fd7d3e 72 #define strcmp_P(a, b) strcmp((a), (b))
khaledelmadawi 0:e63996fd7d3e 73
khaledelmadawi 0:e63996fd7d3e 74 #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
khaledelmadawi 0:e63996fd7d3e 75 #define pgm_read_word(addr) (*(const unsigned short *)(addr))
khaledelmadawi 0:e63996fd7d3e 76 #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
khaledelmadawi 0:e63996fd7d3e 77 #define pgm_read_float(addr) (*(const float *)(addr))
khaledelmadawi 0:e63996fd7d3e 78
khaledelmadawi 0:e63996fd7d3e 79 #define pgm_read_byte_near(addr) pgm_read_byte(addr)
khaledelmadawi 0:e63996fd7d3e 80 #define pgm_read_word_near(addr) pgm_read_word(addr)
khaledelmadawi 0:e63996fd7d3e 81 #define pgm_read_dword_near(addr) pgm_read_dword(addr)
khaledelmadawi 0:e63996fd7d3e 82 #define pgm_read_float_near(addr) pgm_read_float(addr)
khaledelmadawi 0:e63996fd7d3e 83 #define pgm_read_byte_far(addr) pgm_read_byte(addr)
khaledelmadawi 0:e63996fd7d3e 84 #define pgm_read_word_far(addr) pgm_read_word(addr)
khaledelmadawi 0:e63996fd7d3e 85 #define pgm_read_dword_far(addr) pgm_read_dword(addr)
khaledelmadawi 0:e63996fd7d3e 86 #define pgm_read_float_far(addr) pgm_read_float(addr)
khaledelmadawi 0:e63996fd7d3e 87 #endif
khaledelmadawi 0:e63996fd7d3e 88 #endif
khaledelmadawi 0:e63996fd7d3e 89
khaledelmadawi 0:e63996fd7d3e 90 /* Source is from the InvenSense MotionApps v2 demo code. Original source is
khaledelmadawi 0:e63996fd7d3e 91 * unavailable, unless you happen to be amazing as decompiling binary by
khaledelmadawi 0:e63996fd7d3e 92 * hand (in which case, please contact me, and I'm totally serious).
khaledelmadawi 0:e63996fd7d3e 93 *
khaledelmadawi 0:e63996fd7d3e 94 * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
khaledelmadawi 0:e63996fd7d3e 95 * DMP reverse-engineering he did to help make this bit of wizardry
khaledelmadawi 0:e63996fd7d3e 96 * possible.
khaledelmadawi 0:e63996fd7d3e 97 */
khaledelmadawi 0:e63996fd7d3e 98
khaledelmadawi 0:e63996fd7d3e 99 // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
khaledelmadawi 0:e63996fd7d3e 100 // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
khaledelmadawi 0:e63996fd7d3e 101 // after moving string constants to flash memory storage using the F()
khaledelmadawi 0:e63996fd7d3e 102 // compiler macro (Arduino IDE 1.0+ required).
khaledelmadawi 0:e63996fd7d3e 103
khaledelmadawi 0:e63996fd7d3e 104 //#define DEBUG
khaledelmadawi 0:e63996fd7d3e 105 #ifdef DEBUG
khaledelmadawi 0:e63996fd7d3e 106 #define DEBUG_PRINT(x) pc.printf(x) //Serial.print(x)
khaledelmadawi 0:e63996fd7d3e 107 #define DEBUG_PRINTF(x, y) pc.printf(x,y) //Serial.print(x, y)
khaledelmadawi 0:e63996fd7d3e 108
khaledelmadawi 0:e63996fd7d3e 109 #define F(x) x
khaledelmadawi 0:e63996fd7d3e 110 #else
khaledelmadawi 0:e63996fd7d3e 111 #define DEBUG_PRINT(x)
khaledelmadawi 0:e63996fd7d3e 112 #define DEBUG_PRINTF(x, y)
khaledelmadawi 0:e63996fd7d3e 113
khaledelmadawi 0:e63996fd7d3e 114 #endif
khaledelmadawi 0:e63996fd7d3e 115
khaledelmadawi 0:e63996fd7d3e 116 #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[]
khaledelmadawi 0:e63996fd7d3e 117 #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[]
khaledelmadawi 0:e63996fd7d3e 118 #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[]
khaledelmadawi 0:e63996fd7d3e 119
khaledelmadawi 0:e63996fd7d3e 120 /* ================================================================================================ *
khaledelmadawi 0:e63996fd7d3e 121 | Default MotionApps v2.0 42-byte FIFO packet structure: |
khaledelmadawi 0:e63996fd7d3e 122 | |
khaledelmadawi 0:e63996fd7d3e 123 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
khaledelmadawi 0:e63996fd7d3e 124 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
khaledelmadawi 0:e63996fd7d3e 125 | |
khaledelmadawi 0:e63996fd7d3e 126 | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
khaledelmadawi 0:e63996fd7d3e 127 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
khaledelmadawi 0:e63996fd7d3e 128 * ================================================================================================ */
khaledelmadawi 0:e63996fd7d3e 129
khaledelmadawi 0:e63996fd7d3e 130 // this block of memory gets written to the MPU on start-up, and it seems
khaledelmadawi 0:e63996fd7d3e 131 // to be volatile memory, so it has to be done each time (it only takes ~1
khaledelmadawi 0:e63996fd7d3e 132 // second though)
khaledelmadawi 0:e63996fd7d3e 133 const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
khaledelmadawi 0:e63996fd7d3e 134 // bank 0, 256 bytes
khaledelmadawi 0:e63996fd7d3e 135 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 136 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
khaledelmadawi 0:e63996fd7d3e 137 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 138 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 139 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
khaledelmadawi 0:e63996fd7d3e 140 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 141 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 142 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 143 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
khaledelmadawi 0:e63996fd7d3e 144 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
khaledelmadawi 0:e63996fd7d3e 145 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 146 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
khaledelmadawi 0:e63996fd7d3e 147 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 148 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
khaledelmadawi 0:e63996fd7d3e 149 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
khaledelmadawi 0:e63996fd7d3e 150 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
khaledelmadawi 0:e63996fd7d3e 151
khaledelmadawi 0:e63996fd7d3e 152 // bank 1, 256 bytes
khaledelmadawi 0:e63996fd7d3e 153 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 154 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 155 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
khaledelmadawi 0:e63996fd7d3e 156 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
khaledelmadawi 0:e63996fd7d3e 157 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
khaledelmadawi 0:e63996fd7d3e 158 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 159 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
khaledelmadawi 0:e63996fd7d3e 160 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 161 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 162 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 163 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
khaledelmadawi 0:e63996fd7d3e 164 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 165 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
khaledelmadawi 0:e63996fd7d3e 166 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 167 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 168 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
khaledelmadawi 0:e63996fd7d3e 169
khaledelmadawi 0:e63996fd7d3e 170 // bank 2, 256 bytes
khaledelmadawi 0:e63996fd7d3e 171 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 172 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
khaledelmadawi 0:e63996fd7d3e 173 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 174 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 175 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 176 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 177 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 178 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 179 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 180 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 181 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 182 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 183 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 184 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 185 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 186 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 187
khaledelmadawi 0:e63996fd7d3e 188 // bank 3, 256 bytes
khaledelmadawi 0:e63996fd7d3e 189 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
khaledelmadawi 0:e63996fd7d3e 190 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
khaledelmadawi 0:e63996fd7d3e 191 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
khaledelmadawi 0:e63996fd7d3e 192 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
khaledelmadawi 0:e63996fd7d3e 193 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
khaledelmadawi 0:e63996fd7d3e 194 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
khaledelmadawi 0:e63996fd7d3e 195 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
khaledelmadawi 0:e63996fd7d3e 196 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
khaledelmadawi 0:e63996fd7d3e 197 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
khaledelmadawi 0:e63996fd7d3e 198 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
khaledelmadawi 0:e63996fd7d3e 199 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
khaledelmadawi 0:e63996fd7d3e 200 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
khaledelmadawi 0:e63996fd7d3e 201 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
khaledelmadawi 0:e63996fd7d3e 202 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
khaledelmadawi 0:e63996fd7d3e 203 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
khaledelmadawi 0:e63996fd7d3e 204 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
khaledelmadawi 0:e63996fd7d3e 205
khaledelmadawi 0:e63996fd7d3e 206 // bank 4, 256 bytes
khaledelmadawi 0:e63996fd7d3e 207 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
khaledelmadawi 0:e63996fd7d3e 208 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
khaledelmadawi 0:e63996fd7d3e 209 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
khaledelmadawi 0:e63996fd7d3e 210 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
khaledelmadawi 0:e63996fd7d3e 211 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
khaledelmadawi 0:e63996fd7d3e 212 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
khaledelmadawi 0:e63996fd7d3e 213 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
khaledelmadawi 0:e63996fd7d3e 214 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
khaledelmadawi 0:e63996fd7d3e 215 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
khaledelmadawi 0:e63996fd7d3e 216 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
khaledelmadawi 0:e63996fd7d3e 217 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
khaledelmadawi 0:e63996fd7d3e 218 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
khaledelmadawi 0:e63996fd7d3e 219 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
khaledelmadawi 0:e63996fd7d3e 220 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
khaledelmadawi 0:e63996fd7d3e 221 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
khaledelmadawi 0:e63996fd7d3e 222 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
khaledelmadawi 0:e63996fd7d3e 223
khaledelmadawi 0:e63996fd7d3e 224 // bank 5, 256 bytes
khaledelmadawi 0:e63996fd7d3e 225 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
khaledelmadawi 0:e63996fd7d3e 226 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
khaledelmadawi 0:e63996fd7d3e 227 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
khaledelmadawi 0:e63996fd7d3e 228 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
khaledelmadawi 0:e63996fd7d3e 229 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
khaledelmadawi 0:e63996fd7d3e 230 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
khaledelmadawi 0:e63996fd7d3e 231 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
khaledelmadawi 0:e63996fd7d3e 232 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
khaledelmadawi 0:e63996fd7d3e 233 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
khaledelmadawi 0:e63996fd7d3e 234 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
khaledelmadawi 0:e63996fd7d3e 235 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
khaledelmadawi 0:e63996fd7d3e 236 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
khaledelmadawi 0:e63996fd7d3e 237 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
khaledelmadawi 0:e63996fd7d3e 238 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
khaledelmadawi 0:e63996fd7d3e 239 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
khaledelmadawi 0:e63996fd7d3e 240 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
khaledelmadawi 0:e63996fd7d3e 241
khaledelmadawi 0:e63996fd7d3e 242 // bank 6, 256 bytes
khaledelmadawi 0:e63996fd7d3e 243 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
khaledelmadawi 0:e63996fd7d3e 244 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
khaledelmadawi 0:e63996fd7d3e 245 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
khaledelmadawi 0:e63996fd7d3e 246 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
khaledelmadawi 0:e63996fd7d3e 247 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
khaledelmadawi 0:e63996fd7d3e 248 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
khaledelmadawi 0:e63996fd7d3e 249 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
khaledelmadawi 0:e63996fd7d3e 250 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
khaledelmadawi 0:e63996fd7d3e 251 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
khaledelmadawi 0:e63996fd7d3e 252 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
khaledelmadawi 0:e63996fd7d3e 253 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
khaledelmadawi 0:e63996fd7d3e 254 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
khaledelmadawi 0:e63996fd7d3e 255 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
khaledelmadawi 0:e63996fd7d3e 256 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
khaledelmadawi 0:e63996fd7d3e 257 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
khaledelmadawi 0:e63996fd7d3e 258 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
khaledelmadawi 0:e63996fd7d3e 259
khaledelmadawi 0:e63996fd7d3e 260 // bank 7, 138 bytes (remainder)
khaledelmadawi 0:e63996fd7d3e 261 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
khaledelmadawi 0:e63996fd7d3e 262 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
khaledelmadawi 0:e63996fd7d3e 263 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
khaledelmadawi 0:e63996fd7d3e 264 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
khaledelmadawi 0:e63996fd7d3e 265 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
khaledelmadawi 0:e63996fd7d3e 266 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
khaledelmadawi 0:e63996fd7d3e 267 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
khaledelmadawi 0:e63996fd7d3e 268 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
khaledelmadawi 0:e63996fd7d3e 269 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
khaledelmadawi 0:e63996fd7d3e 270 };
khaledelmadawi 0:e63996fd7d3e 271
khaledelmadawi 0:e63996fd7d3e 272 // thanks to Noah Zerkin for piecing this stuff together!
khaledelmadawi 0:e63996fd7d3e 273 const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
khaledelmadawi 0:e63996fd7d3e 274 // BANK OFFSET LENGTH [DATA]
khaledelmadawi 0:e63996fd7d3e 275 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
khaledelmadawi 0:e63996fd7d3e 276 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
khaledelmadawi 0:e63996fd7d3e 277 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration
khaledelmadawi 0:e63996fd7d3e 278 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration
khaledelmadawi 0:e63996fd7d3e 279 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
khaledelmadawi 0:e63996fd7d3e 280 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration
khaledelmadawi 0:e63996fd7d3e 281 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration
khaledelmadawi 0:e63996fd7d3e 282 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration
khaledelmadawi 0:e63996fd7d3e 283 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
khaledelmadawi 0:e63996fd7d3e 284 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01
khaledelmadawi 0:e63996fd7d3e 285 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
khaledelmadawi 0:e63996fd7d3e 286 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10
khaledelmadawi 0:e63996fd7d3e 287 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
khaledelmadawi 0:e63996fd7d3e 288 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
khaledelmadawi 0:e63996fd7d3e 289 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
khaledelmadawi 0:e63996fd7d3e 290 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
khaledelmadawi 0:e63996fd7d3e 291 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22
khaledelmadawi 0:e63996fd7d3e 292 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
khaledelmadawi 0:e63996fd7d3e 293 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
khaledelmadawi 0:e63996fd7d3e 294 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
khaledelmadawi 0:e63996fd7d3e 295 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
khaledelmadawi 0:e63996fd7d3e 296 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone
khaledelmadawi 0:e63996fd7d3e 297 // SPECIAL 0x01 = enable interrupts
khaledelmadawi 0:e63996fd7d3e 298 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
khaledelmadawi 0:e63996fd7d3e 299 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt
khaledelmadawi 0:e63996fd7d3e 300 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
khaledelmadawi 0:e63996fd7d3e 301 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer
khaledelmadawi 0:e63996fd7d3e 302 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
khaledelmadawi 0:e63996fd7d3e 303 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
khaledelmadawi 0:e63996fd7d3e 304 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
khaledelmadawi 0:e63996fd7d3e 305 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate
khaledelmadawi 0:e63996fd7d3e 306
khaledelmadawi 0:e63996fd7d3e 307 // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
khaledelmadawi 0:e63996fd7d3e 308 // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
khaledelmadawi 0:e63996fd7d3e 309 // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
khaledelmadawi 0:e63996fd7d3e 310
khaledelmadawi 0:e63996fd7d3e 311 // It is important to make sure the host processor can keep up with reading and processing
khaledelmadawi 0:e63996fd7d3e 312 // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
khaledelmadawi 0:e63996fd7d3e 313 };
khaledelmadawi 0:e63996fd7d3e 314
khaledelmadawi 0:e63996fd7d3e 315 const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
khaledelmadawi 0:e63996fd7d3e 316 0x01, 0xB2, 0x02, 0xFF, 0xFF,
khaledelmadawi 0:e63996fd7d3e 317 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
khaledelmadawi 0:e63996fd7d3e 318 0x01, 0x6A, 0x02, 0x06, 0x00,
khaledelmadawi 0:e63996fd7d3e 319 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 320 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 321 0x01, 0x62, 0x02, 0x00, 0x00,
khaledelmadawi 0:e63996fd7d3e 322 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
khaledelmadawi 0:e63996fd7d3e 323 };
khaledelmadawi 0:e63996fd7d3e 324
khaledelmadawi 0:e63996fd7d3e 325 uint8_t MPU6050::dmpInitialize() {
khaledelmadawi 0:e63996fd7d3e 326 // reset device
khaledelmadawi 0:e63996fd7d3e 327 Serial pc(USBTX, USBRX); // tx, rx
khaledelmadawi 0:e63996fd7d3e 328 pc.baud(115200);
khaledelmadawi 0:e63996fd7d3e 329 wait_ms(50);
khaledelmadawi 0:e63996fd7d3e 330
khaledelmadawi 0:e63996fd7d3e 331 pc.printf("\n\nSet USB Baudrate to 115200...\n");
khaledelmadawi 0:e63996fd7d3e 332
khaledelmadawi 0:e63996fd7d3e 333 pc.printf("\n\nResetting MPU6050...\n");
khaledelmadawi 0:e63996fd7d3e 334 reset();
khaledelmadawi 0:e63996fd7d3e 335
khaledelmadawi 0:e63996fd7d3e 336 wait_ms(30);
khaledelmadawi 0:e63996fd7d3e 337
khaledelmadawi 0:e63996fd7d3e 338 // enable sleep mode and wake cycle
khaledelmadawi 0:e63996fd7d3e 339 /*Serial.println(F("Enabling sleep mode..."));
khaledelmadawi 0:e63996fd7d3e 340 setSleepEnabled(true);
khaledelmadawi 0:e63996fd7d3e 341 Serial.println(F("Enabling wake cycle..."));
khaledelmadawi 0:e63996fd7d3e 342 setWakeCycleEnabled(true);*/
khaledelmadawi 0:e63996fd7d3e 343
khaledelmadawi 0:e63996fd7d3e 344 // disable sleep mode
khaledelmadawi 0:e63996fd7d3e 345 DEBUG_PRINT("Disabling sleep mode...\n");
khaledelmadawi 0:e63996fd7d3e 346 setSleepEnabled(false);
khaledelmadawi 0:e63996fd7d3e 347
khaledelmadawi 0:e63996fd7d3e 348 // get MPU hardware revision
khaledelmadawi 0:e63996fd7d3e 349 DEBUG_PRINT("Selecting user bank 16...\n");
khaledelmadawi 0:e63996fd7d3e 350 setMemoryBank(0x10, true, true);
khaledelmadawi 0:e63996fd7d3e 351 DEBUG_PRINT("Selecting memory byte 6...\n");
khaledelmadawi 0:e63996fd7d3e 352 setMemoryStartAddress(0x06);
khaledelmadawi 0:e63996fd7d3e 353 DEBUG_PRINT("Checking hardware revision...\n");
khaledelmadawi 0:e63996fd7d3e 354 uint8_t hwRevision = readMemoryByte();
khaledelmadawi 0:e63996fd7d3e 355 DEBUG_PRINT("Revision @ user[16][6] = ");
khaledelmadawi 0:e63996fd7d3e 356 DEBUG_PRINTF("%x\n",hwRevision);
khaledelmadawi 0:e63996fd7d3e 357 DEBUG_PRINT("Resetting memory bank selection to 0...\n");
khaledelmadawi 0:e63996fd7d3e 358 setMemoryBank(0, false, false);
khaledelmadawi 0:e63996fd7d3e 359
khaledelmadawi 0:e63996fd7d3e 360 // check OTP bank valid
khaledelmadawi 0:e63996fd7d3e 361 DEBUG_PRINT("Reading OTP bank valid flag...\n");
khaledelmadawi 0:e63996fd7d3e 362 uint8_t otpValid = getOTPBankValid();
khaledelmadawi 0:e63996fd7d3e 363
khaledelmadawi 0:e63996fd7d3e 364 DEBUG_PRINT("OTP bank is ");
khaledelmadawi 0:e63996fd7d3e 365 if(otpValid) DEBUG_PRINT("valid!\n");
khaledelmadawi 0:e63996fd7d3e 366 else DEBUG_PRINT("invalid!\n");
khaledelmadawi 0:e63996fd7d3e 367
khaledelmadawi 0:e63996fd7d3e 368 // get X/Y/Z gyro offsets
khaledelmadawi 0:e63996fd7d3e 369 /*
khaledelmadawi 0:e63996fd7d3e 370 DEBUG_PRINT("\nReading gyro offset TC values...\n");
khaledelmadawi 0:e63996fd7d3e 371 int8_t xgOffsetTC = mpu.getXGyroOffsetTC();
khaledelmadawi 0:e63996fd7d3e 372 int8_t ygOffsetTC = getYGyroOffsetTC();
khaledelmadawi 0:e63996fd7d3e 373 int8_t zgOffsetTC = getZGyroOffsetTC();
khaledelmadawi 0:e63996fd7d3e 374 DEBUG_PRINTF("X gyro offset = %u\n",xgOffset);
khaledelmadawi 0:e63996fd7d3e 375 DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset);
khaledelmadawi 0:e63996fd7d3e 376 DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset);
khaledelmadawi 0:e63996fd7d3e 377 */
khaledelmadawi 0:e63996fd7d3e 378 // setup weird slave stuff (?)
khaledelmadawi 0:e63996fd7d3e 379 DEBUG_PRINT("Setting slave 0 address to 0x7F...\n");
khaledelmadawi 0:e63996fd7d3e 380 setSlaveAddress(0, 0x7F);
khaledelmadawi 0:e63996fd7d3e 381
khaledelmadawi 0:e63996fd7d3e 382 DEBUG_PRINT("Disabling I2C Master mode...");
khaledelmadawi 0:e63996fd7d3e 383 setI2CMasterModeEnabled(false);
khaledelmadawi 0:e63996fd7d3e 384 DEBUG_PRINT("Setting slave 0 address to 0x68 (self)...");
khaledelmadawi 0:e63996fd7d3e 385 setSlaveAddress(0, 0x68);
khaledelmadawi 0:e63996fd7d3e 386 DEBUG_PRINT("Resetting I2C Master control...\n");
khaledelmadawi 0:e63996fd7d3e 387 resetI2CMaster();
khaledelmadawi 0:e63996fd7d3e 388
khaledelmadawi 0:e63996fd7d3e 389 wait_ms(20);
khaledelmadawi 0:e63996fd7d3e 390
khaledelmadawi 0:e63996fd7d3e 391 // load DMP code into memory banks
khaledelmadawi 0:e63996fd7d3e 392 DEBUG_PRINT("Writing DMP code to MPU memory banks (");
khaledelmadawi 0:e63996fd7d3e 393 DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
khaledelmadawi 0:e63996fd7d3e 394 DEBUG_PRINT(" bytes)\n");
khaledelmadawi 0:e63996fd7d3e 395 if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
khaledelmadawi 0:e63996fd7d3e 396 DEBUG_PRINT("Success! DMP code written and verified.\n");
khaledelmadawi 0:e63996fd7d3e 397
khaledelmadawi 0:e63996fd7d3e 398 // write DMP configuration
khaledelmadawi 0:e63996fd7d3e 399 DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
khaledelmadawi 0:e63996fd7d3e 400 DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE);
khaledelmadawi 0:e63996fd7d3e 401 DEBUG_PRINT(" bytes in config def)\n");
khaledelmadawi 0:e63996fd7d3e 402 if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
khaledelmadawi 0:e63996fd7d3e 403 DEBUG_PRINT("Success! DMP configuration written and verified.\n");
khaledelmadawi 0:e63996fd7d3e 404
khaledelmadawi 0:e63996fd7d3e 405 DEBUG_PRINT("Setting clock source to Z Gyro...\n");
khaledelmadawi 0:e63996fd7d3e 406 setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
khaledelmadawi 0:e63996fd7d3e 407
khaledelmadawi 0:e63996fd7d3e 408 DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
khaledelmadawi 0:e63996fd7d3e 409 setIntEnabled(0x12);
khaledelmadawi 0:e63996fd7d3e 410
khaledelmadawi 0:e63996fd7d3e 411 DEBUG_PRINT("Setting sample rate to 200Hz...");
khaledelmadawi 0:e63996fd7d3e 412 setRate(4); // 1khz / (1 + 4) = 200 Hz
khaledelmadawi 0:e63996fd7d3e 413
khaledelmadawi 0:e63996fd7d3e 414 DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
khaledelmadawi 0:e63996fd7d3e 415 setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
khaledelmadawi 0:e63996fd7d3e 416
khaledelmadawi 0:e63996fd7d3e 417 DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
khaledelmadawi 0:e63996fd7d3e 418 setDLPFMode(MPU6050_DLPF_BW_42);
khaledelmadawi 0:e63996fd7d3e 419
khaledelmadawi 0:e63996fd7d3e 420 DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
khaledelmadawi 0:e63996fd7d3e 421 setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
khaledelmadawi 0:e63996fd7d3e 422
khaledelmadawi 0:e63996fd7d3e 423 DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 424 setDMPConfig1(0x03);
khaledelmadawi 0:e63996fd7d3e 425 setDMPConfig2(0x00);
khaledelmadawi 0:e63996fd7d3e 426
khaledelmadawi 0:e63996fd7d3e 427 DEBUG_PRINT("Clearing OTP Bank flag...");
khaledelmadawi 0:e63996fd7d3e 428 setOTPBankValid(false);
khaledelmadawi 0:e63996fd7d3e 429
khaledelmadawi 0:e63996fd7d3e 430 DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n");
khaledelmadawi 0:e63996fd7d3e 431 //setXGyroOffsetTC(xgOffsetTC);
khaledelmadawi 0:e63996fd7d3e 432 //setYGyroOffsetTC(ygOffsetTC);
khaledelmadawi 0:e63996fd7d3e 433 //setZGyroOffsetTC(zgOffsetTC);
khaledelmadawi 0:e63996fd7d3e 434
khaledelmadawi 0:e63996fd7d3e 435 //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
khaledelmadawi 0:e63996fd7d3e 436 //setXGyroOffset(0);
khaledelmadawi 0:e63996fd7d3e 437 //setYGyroOffset(0);
khaledelmadawi 0:e63996fd7d3e 438 //setZGyroOffset(0);
khaledelmadawi 0:e63996fd7d3e 439
khaledelmadawi 0:e63996fd7d3e 440 DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 441 uint8_t dmpUpdate[16], j;
khaledelmadawi 0:e63996fd7d3e 442 uint16_t pos = 0;
khaledelmadawi 0:e63996fd7d3e 443 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 444 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 445
khaledelmadawi 0:e63996fd7d3e 446 DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 447 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 448 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 449
khaledelmadawi 0:e63996fd7d3e 450 DEBUG_PRINT("Resetting FIFO...\n");
khaledelmadawi 0:e63996fd7d3e 451 resetFIFO();
khaledelmadawi 0:e63996fd7d3e 452
khaledelmadawi 0:e63996fd7d3e 453 DEBUG_PRINT("Reading FIFO count...\n");
khaledelmadawi 0:e63996fd7d3e 454 uint16_t fifoCount = getFIFOCount();
khaledelmadawi 0:e63996fd7d3e 455 uint8_t fifoBuffer[128];
khaledelmadawi 0:e63996fd7d3e 456
khaledelmadawi 0:e63996fd7d3e 457 DEBUG_PRINT("Current FIFO count=");
khaledelmadawi 0:e63996fd7d3e 458 DEBUG_PRINTF("%u\n",fifoCount);
khaledelmadawi 0:e63996fd7d3e 459 getFIFOBytes(fifoBuffer, fifoCount);
khaledelmadawi 0:e63996fd7d3e 460
khaledelmadawi 0:e63996fd7d3e 461 DEBUG_PRINT("Setting motion detection threshold to 2...\n");
khaledelmadawi 0:e63996fd7d3e 462 setMotionDetectionThreshold(2);
khaledelmadawi 0:e63996fd7d3e 463
khaledelmadawi 0:e63996fd7d3e 464 DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
khaledelmadawi 0:e63996fd7d3e 465 setZeroMotionDetectionThreshold(156);
khaledelmadawi 0:e63996fd7d3e 466
khaledelmadawi 0:e63996fd7d3e 467 DEBUG_PRINT("Setting motion detection duration to 80...");
khaledelmadawi 0:e63996fd7d3e 468 setMotionDetectionDuration(80);
khaledelmadawi 0:e63996fd7d3e 469
khaledelmadawi 0:e63996fd7d3e 470 DEBUG_PRINT("Setting zero-motion detection duration to 0...");
khaledelmadawi 0:e63996fd7d3e 471 setZeroMotionDetectionDuration(0);
khaledelmadawi 0:e63996fd7d3e 472
khaledelmadawi 0:e63996fd7d3e 473 DEBUG_PRINT("Resetting FIFO...\n");
khaledelmadawi 0:e63996fd7d3e 474 resetFIFO();
khaledelmadawi 0:e63996fd7d3e 475
khaledelmadawi 0:e63996fd7d3e 476 DEBUG_PRINT("Enabling FIFO...\n");
khaledelmadawi 0:e63996fd7d3e 477 setFIFOEnabled(true);
khaledelmadawi 0:e63996fd7d3e 478
khaledelmadawi 0:e63996fd7d3e 479 DEBUG_PRINT("Enabling DMP...\n");
khaledelmadawi 0:e63996fd7d3e 480 setDMPEnabled(true);
khaledelmadawi 0:e63996fd7d3e 481
khaledelmadawi 0:e63996fd7d3e 482 DEBUG_PRINT("Resetting DMP...\n");
khaledelmadawi 0:e63996fd7d3e 483 resetDMP();
khaledelmadawi 0:e63996fd7d3e 484
khaledelmadawi 0:e63996fd7d3e 485 DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 486 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 487 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 488
khaledelmadawi 0:e63996fd7d3e 489 DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 490 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 491 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 492
khaledelmadawi 0:e63996fd7d3e 493 DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 494 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 495 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 496
khaledelmadawi 0:e63996fd7d3e 497 DEBUG_PRINT("Waiting for FIFO count > 2...\n");
khaledelmadawi 0:e63996fd7d3e 498 while ((fifoCount = getFIFOCount()) < 3);
khaledelmadawi 0:e63996fd7d3e 499
khaledelmadawi 0:e63996fd7d3e 500 DEBUG_PRINT("Current FIFO count=");
khaledelmadawi 0:e63996fd7d3e 501 DEBUG_PRINTF("%u\n",fifoCount);
khaledelmadawi 0:e63996fd7d3e 502 DEBUG_PRINT("Reading FIFO data...\n");
khaledelmadawi 0:e63996fd7d3e 503 getFIFOBytes(fifoBuffer, fifoCount);
khaledelmadawi 0:e63996fd7d3e 504
khaledelmadawi 0:e63996fd7d3e 505 DEBUG_PRINT("Reading interrupt status...\n");
khaledelmadawi 0:e63996fd7d3e 506 uint8_t mpuIntStatus = getIntStatus();
khaledelmadawi 0:e63996fd7d3e 507
khaledelmadawi 0:e63996fd7d3e 508 DEBUG_PRINT("Current interrupt status=");
khaledelmadawi 0:e63996fd7d3e 509 DEBUG_PRINTF("%x\n",mpuIntStatus);
khaledelmadawi 0:e63996fd7d3e 510
khaledelmadawi 0:e63996fd7d3e 511 DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n");
khaledelmadawi 0:e63996fd7d3e 512 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 513 readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 514
khaledelmadawi 0:e63996fd7d3e 515 DEBUG_PRINT("Waiting for FIFO count > 2...\n");
khaledelmadawi 0:e63996fd7d3e 516 while ((fifoCount = getFIFOCount()) < 3);
khaledelmadawi 0:e63996fd7d3e 517
khaledelmadawi 0:e63996fd7d3e 518 DEBUG_PRINT("Current FIFO count=");
khaledelmadawi 0:e63996fd7d3e 519 DEBUG_PRINTF("%u\n",fifoCount);
khaledelmadawi 0:e63996fd7d3e 520
khaledelmadawi 0:e63996fd7d3e 521 DEBUG_PRINT("Reading FIFO data...\n");
khaledelmadawi 0:e63996fd7d3e 522 getFIFOBytes(fifoBuffer, fifoCount);
khaledelmadawi 0:e63996fd7d3e 523
khaledelmadawi 0:e63996fd7d3e 524 DEBUG_PRINT("Reading interrupt status...\n");
khaledelmadawi 0:e63996fd7d3e 525 mpuIntStatus = getIntStatus();
khaledelmadawi 0:e63996fd7d3e 526
khaledelmadawi 0:e63996fd7d3e 527 DEBUG_PRINT("Current interrupt status=");
khaledelmadawi 0:e63996fd7d3e 528 DEBUG_PRINTF("%x\n",mpuIntStatus);
khaledelmadawi 0:e63996fd7d3e 529
khaledelmadawi 0:e63996fd7d3e 530 DEBUG_PRINT("Writing final memory update 7/7 (function unknown)...");
khaledelmadawi 0:e63996fd7d3e 531 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
khaledelmadawi 0:e63996fd7d3e 532 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
khaledelmadawi 0:e63996fd7d3e 533
khaledelmadawi 0:e63996fd7d3e 534 DEBUG_PRINT("DMP is good to go! Finally.\n");
khaledelmadawi 0:e63996fd7d3e 535
khaledelmadawi 0:e63996fd7d3e 536 DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
khaledelmadawi 0:e63996fd7d3e 537 setDMPEnabled(false);
khaledelmadawi 0:e63996fd7d3e 538
khaledelmadawi 0:e63996fd7d3e 539 DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n");
khaledelmadawi 0:e63996fd7d3e 540 dmpPacketSize = 42;
khaledelmadawi 0:e63996fd7d3e 541 /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
khaledelmadawi 0:e63996fd7d3e 542 return 3; // TODO: proper error code for no memory
khaledelmadawi 0:e63996fd7d3e 543 }*/
khaledelmadawi 0:e63996fd7d3e 544
khaledelmadawi 0:e63996fd7d3e 545 DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
khaledelmadawi 0:e63996fd7d3e 546 resetFIFO();
khaledelmadawi 0:e63996fd7d3e 547 getIntStatus();
khaledelmadawi 0:e63996fd7d3e 548 } else {
khaledelmadawi 0:e63996fd7d3e 549 DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
khaledelmadawi 0:e63996fd7d3e 550 return 2; // configuration block loading failed
khaledelmadawi 0:e63996fd7d3e 551 }
khaledelmadawi 0:e63996fd7d3e 552 } else {
khaledelmadawi 0:e63996fd7d3e 553 DEBUG_PRINT("ERROR! DMP code verification failed.");
khaledelmadawi 0:e63996fd7d3e 554 return 1; // main binary block loading failed
khaledelmadawi 0:e63996fd7d3e 555 }
khaledelmadawi 0:e63996fd7d3e 556 return 0; // success
khaledelmadawi 0:e63996fd7d3e 557 }
khaledelmadawi 0:e63996fd7d3e 558
khaledelmadawi 0:e63996fd7d3e 559 bool MPU6050::dmpPacketAvailable() {
khaledelmadawi 0:e63996fd7d3e 560 return getFIFOCount() >= dmpGetFIFOPacketSize();
khaledelmadawi 0:e63996fd7d3e 561 }
khaledelmadawi 0:e63996fd7d3e 562
khaledelmadawi 0:e63996fd7d3e 563 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
khaledelmadawi 0:e63996fd7d3e 564 // uint8_t MPU6050::dmpGetFIFORate();
khaledelmadawi 0:e63996fd7d3e 565 // uint8_t MPU6050::dmpGetSampleStepSizeMS();
khaledelmadawi 0:e63996fd7d3e 566 // uint8_t MPU6050::dmpGetSampleFrequency();
khaledelmadawi 0:e63996fd7d3e 567 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
khaledelmadawi 0:e63996fd7d3e 568
khaledelmadawi 0:e63996fd7d3e 569 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
khaledelmadawi 0:e63996fd7d3e 570 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
khaledelmadawi 0:e63996fd7d3e 571 //uint8_t MPU6050::dmpRunFIFORateProcesses();
khaledelmadawi 0:e63996fd7d3e 572
khaledelmadawi 0:e63996fd7d3e 573 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 574 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 575 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 576 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 577 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 578 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 579 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 580 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 581 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 582 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 583 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 584 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
khaledelmadawi 0:e63996fd7d3e 585
khaledelmadawi 0:e63996fd7d3e 586 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 587 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 588 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 589 data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]);
khaledelmadawi 0:e63996fd7d3e 590 data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]);
khaledelmadawi 0:e63996fd7d3e 591 data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]);
khaledelmadawi 0:e63996fd7d3e 592 return 0;
khaledelmadawi 0:e63996fd7d3e 593 }
khaledelmadawi 0:e63996fd7d3e 594 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 595 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 596 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 597 data[0] = (packet[28] << 8) + packet[29];
khaledelmadawi 0:e63996fd7d3e 598 data[1] = (packet[32] << 8) + packet[33];
khaledelmadawi 0:e63996fd7d3e 599 data[2] = (packet[36] << 8) + packet[37];
khaledelmadawi 0:e63996fd7d3e 600 return 0;
khaledelmadawi 0:e63996fd7d3e 601 }
khaledelmadawi 0:e63996fd7d3e 602 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 603 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 604 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 605 v -> x = (packet[28] << 8) + packet[29];
khaledelmadawi 0:e63996fd7d3e 606 v -> y = (packet[32] << 8) + packet[33];
khaledelmadawi 0:e63996fd7d3e 607 v -> z = (packet[36] << 8) + packet[37];
khaledelmadawi 0:e63996fd7d3e 608 return 0;
khaledelmadawi 0:e63996fd7d3e 609 }
khaledelmadawi 0:e63996fd7d3e 610 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 611 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 612 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 613 data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
khaledelmadawi 0:e63996fd7d3e 614 data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
khaledelmadawi 0:e63996fd7d3e 615 data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
khaledelmadawi 0:e63996fd7d3e 616 data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
khaledelmadawi 0:e63996fd7d3e 617 return 0;
khaledelmadawi 0:e63996fd7d3e 618 }
khaledelmadawi 0:e63996fd7d3e 619 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 620 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 621 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 622 data[0] = ((packet[0] << 8) + packet[1]);
khaledelmadawi 0:e63996fd7d3e 623 data[1] = ((packet[4] << 8) + packet[5]);
khaledelmadawi 0:e63996fd7d3e 624 data[2] = ((packet[8] << 8) + packet[9]);
khaledelmadawi 0:e63996fd7d3e 625 data[3] = ((packet[12] << 8) + packet[13]);
khaledelmadawi 0:e63996fd7d3e 626 return 0;
khaledelmadawi 0:e63996fd7d3e 627 }
khaledelmadawi 0:e63996fd7d3e 628 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 629 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 630 int16_t qI[4];
khaledelmadawi 0:e63996fd7d3e 631 uint8_t status = dmpGetQuaternion(qI, packet);
khaledelmadawi 0:e63996fd7d3e 632 if (status == 0) {
khaledelmadawi 0:e63996fd7d3e 633 q -> w = (float)qI[0] / 16384.0f;
khaledelmadawi 0:e63996fd7d3e 634 q -> x = (float)qI[1] / 16384.0f;
khaledelmadawi 0:e63996fd7d3e 635 q -> y = (float)qI[2] / 16384.0f;
khaledelmadawi 0:e63996fd7d3e 636 q -> z = (float)qI[3] / 16384.0f;
khaledelmadawi 0:e63996fd7d3e 637 return 0;
khaledelmadawi 0:e63996fd7d3e 638 }
khaledelmadawi 0:e63996fd7d3e 639 return status; // int16 return value, indicates error if this line is reached
khaledelmadawi 0:e63996fd7d3e 640 }
khaledelmadawi 0:e63996fd7d3e 641 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 642 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 643 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 644 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 645 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 646 data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
khaledelmadawi 0:e63996fd7d3e 647 data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
khaledelmadawi 0:e63996fd7d3e 648 data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
khaledelmadawi 0:e63996fd7d3e 649 return 0;
khaledelmadawi 0:e63996fd7d3e 650 }
khaledelmadawi 0:e63996fd7d3e 651 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
khaledelmadawi 0:e63996fd7d3e 652 // TODO: accommodate different arrangements of sent data (ONLY default supported now)
khaledelmadawi 0:e63996fd7d3e 653 if (packet == 0) packet = dmpPacketBuffer;
khaledelmadawi 0:e63996fd7d3e 654 data[0] = (packet[16] << 8) + packet[17];
khaledelmadawi 0:e63996fd7d3e 655 data[1] = (packet[20] << 8) + packet[21];
khaledelmadawi 0:e63996fd7d3e 656 data[2] = (packet[24] << 8) + packet[25];
khaledelmadawi 0:e63996fd7d3e 657 return 0;
khaledelmadawi 0:e63996fd7d3e 658 }
khaledelmadawi 0:e63996fd7d3e 659 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
khaledelmadawi 0:e63996fd7d3e 660 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 661 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
khaledelmadawi 0:e63996fd7d3e 662 // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
khaledelmadawi 0:e63996fd7d3e 663 v -> x = vRaw -> x - gravity -> x*8192;
khaledelmadawi 0:e63996fd7d3e 664 v -> y = vRaw -> y - gravity -> y*8192;
khaledelmadawi 0:e63996fd7d3e 665 v -> z = vRaw -> z - gravity -> z*8192;
khaledelmadawi 0:e63996fd7d3e 666 return 0;
khaledelmadawi 0:e63996fd7d3e 667 }
khaledelmadawi 0:e63996fd7d3e 668 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 669 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
khaledelmadawi 0:e63996fd7d3e 670 // rotate measured 3D acceleration vector into original state
khaledelmadawi 0:e63996fd7d3e 671 // frame of reference based on orientation quaternion
khaledelmadawi 0:e63996fd7d3e 672 memcpy(v, vReal, sizeof(VectorInt16));
khaledelmadawi 0:e63996fd7d3e 673 v -> rotate(q);
khaledelmadawi 0:e63996fd7d3e 674 return 0;
khaledelmadawi 0:e63996fd7d3e 675 }
khaledelmadawi 0:e63996fd7d3e 676 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 677 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 678 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 679 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 680 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 681 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
khaledelmadawi 0:e63996fd7d3e 682 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
khaledelmadawi 0:e63996fd7d3e 683 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
khaledelmadawi 0:e63996fd7d3e 684 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
khaledelmadawi 0:e63996fd7d3e 685 return 0;
khaledelmadawi 0:e63996fd7d3e 686 }
khaledelmadawi 0:e63996fd7d3e 687 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 688 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 689 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 690 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 691
khaledelmadawi 0:e63996fd7d3e 692 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
khaledelmadawi 0:e63996fd7d3e 693 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi
khaledelmadawi 0:e63996fd7d3e 694 data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
khaledelmadawi 0:e63996fd7d3e 695 data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
khaledelmadawi 0:e63996fd7d3e 696 return 0;
khaledelmadawi 0:e63996fd7d3e 697 }
khaledelmadawi 0:e63996fd7d3e 698
khaledelmadawi 0:e63996fd7d3e 699 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
khaledelmadawi 0:e63996fd7d3e 700 // yaw: (about Z axis)
khaledelmadawi 0:e63996fd7d3e 701 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
khaledelmadawi 0:e63996fd7d3e 702 // pitch: (nose up/down, about Y axis)
khaledelmadawi 0:e63996fd7d3e 703 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
khaledelmadawi 0:e63996fd7d3e 704 // roll: (tilt left/right, about X axis)
khaledelmadawi 0:e63996fd7d3e 705 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
khaledelmadawi 0:e63996fd7d3e 706 return 0;
khaledelmadawi 0:e63996fd7d3e 707 }
khaledelmadawi 0:e63996fd7d3e 708
khaledelmadawi 0:e63996fd7d3e 709 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 710 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
khaledelmadawi 0:e63996fd7d3e 711
khaledelmadawi 0:e63996fd7d3e 712 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
khaledelmadawi 0:e63996fd7d3e 713 /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
khaledelmadawi 0:e63996fd7d3e 714 if (dmpData[k] < 0x10) Serial.print("0");
khaledelmadawi 0:e63996fd7d3e 715 Serial.print(dmpData[k], HEX);
khaledelmadawi 0:e63996fd7d3e 716 Serial.print(" ");
khaledelmadawi 0:e63996fd7d3e 717 }
khaledelmadawi 0:e63996fd7d3e 718 Serial.print("\n");*/
khaledelmadawi 0:e63996fd7d3e 719 //Serial.println((uint16_t)dmpPacketBuffer);
khaledelmadawi 0:e63996fd7d3e 720 return 0;
khaledelmadawi 0:e63996fd7d3e 721 }
khaledelmadawi 0:e63996fd7d3e 722 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
khaledelmadawi 0:e63996fd7d3e 723 uint8_t status;
khaledelmadawi 0:e63996fd7d3e 724 uint8_t buf[dmpPacketSize];
khaledelmadawi 0:e63996fd7d3e 725 for (uint8_t i = 0; i < numPackets; i++) {
khaledelmadawi 0:e63996fd7d3e 726 // read packet from FIFO
khaledelmadawi 0:e63996fd7d3e 727 getFIFOBytes(buf, dmpPacketSize);
khaledelmadawi 0:e63996fd7d3e 728
khaledelmadawi 0:e63996fd7d3e 729 // process packet
khaledelmadawi 0:e63996fd7d3e 730 if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
khaledelmadawi 0:e63996fd7d3e 731
khaledelmadawi 0:e63996fd7d3e 732 // increment external process count variable, if supplied
khaledelmadawi 0:e63996fd7d3e 733 if (processed != 0) *processed++;
khaledelmadawi 0:e63996fd7d3e 734 }
khaledelmadawi 0:e63996fd7d3e 735 return 0;
khaledelmadawi 0:e63996fd7d3e 736 }
khaledelmadawi 0:e63996fd7d3e 737
khaledelmadawi 0:e63996fd7d3e 738 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
khaledelmadawi 0:e63996fd7d3e 739
khaledelmadawi 0:e63996fd7d3e 740 // uint8_t MPU6050::dmpInitFIFOParam();
khaledelmadawi 0:e63996fd7d3e 741 // uint8_t MPU6050::dmpCloseFIFO();
khaledelmadawi 0:e63996fd7d3e 742 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
khaledelmadawi 0:e63996fd7d3e 743 // uint8_t MPU6050::dmpDecodeQuantizedAccel();
khaledelmadawi 0:e63996fd7d3e 744 // uint32_t MPU6050::dmpGetGyroSumOfSquare();
khaledelmadawi 0:e63996fd7d3e 745 // uint32_t MPU6050::dmpGetAccelSumOfSquare();
khaledelmadawi 0:e63996fd7d3e 746 // void MPU6050::dmpOverrideQuaternion(long *q);
khaledelmadawi 0:e63996fd7d3e 747 uint16_t MPU6050::dmpGetFIFOPacketSize() {
khaledelmadawi 0:e63996fd7d3e 748 return dmpPacketSize;
khaledelmadawi 0:e63996fd7d3e 749 }
khaledelmadawi 0:e63996fd7d3e 750
khaledelmadawi 0:e63996fd7d3e 751 #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */