quadcopter cufe / Mbed 2 deprecated MPU_6050_nadafa

mpu6050 nadafa

Dependencies:   mbed

MPU6050_9Axis_MotionApps41.h@0:8fe8d6dd7cd6, 2015-06-23 (annotated)

Committer:
khaledelmadawi
Date:
Tue Jun 23 14:47:52 2015 +0000
Revision:
0:8fe8d6dd7cd6
mpu nadafa

Who changed what in which revision?

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