Rolf Ziegler / Mbed 2 deprecated MPU6050-DMP_6DOF

Complete MPU6050dmp example working with processing 2.1 teapot code v1.0 Jan. 2014

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

MPU6050_9Axis_MotionApps41.h@6:029b4b8111ef, 2014-01-10 (annotated)

Committer:
rolfz
Date:
Fri Jan 10 17:01:28 2014 +0000
Revision:
6:029b4b8111ef
Complete MBED example with main code, working with Processing v2.1 Teapot example!

Who changed what in which revision?

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