quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

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