Carter Sharer / MPU6050_V3

This one compatable with brobot V3. first commit to BroBot

Dependencies:   I2Cdev

Dependents:   BroBot_ESE350_Skeleton

Fork of MPU6050 by Carter Sharer

JJ_MPU6050_DMP_6Axis.h@13:241bea255ef7, 2016-12-17 (annotated)

Committer:
csharer
Date:
Sat Dec 17 23:25:55 2016 +0000
Revision:
13:241bea255ef7
Parent:
11:96a82e638330 
Working Stability and Position Controller

Who changed what in which revision?

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