Amr El-Gendy / Mbed 2 deprecated QUAD_PROJECT

a quadcopter code

Dependencies:   Pulse RangeFinder mbed

MPU6050_6Axis_MotionApps20.h@0:4a55d0a21ea9, 2015-11-24 (annotated)

Committer:
Gendy
Date:
Tue Nov 24 19:57:06 2015 +0000
Revision:
0:4a55d0a21ea9
Quad_project PID on angle;

Who changed what in which revision?

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