mirror of https://github.com/arendst/Tasmota.git
855 lines
47 KiB
C
855 lines
47 KiB
C
// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation
|
|
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
|
|
// 6/18/2012 by Jeff Rowberg <jeff@rowberg.net>
|
|
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
|
//
|
|
// Adapted for Sonoff-Tasmota by Oliver Welter <contact@verbotene.zone> 02-04-2018
|
|
//
|
|
// Changelog:
|
|
// ... - ongoing debug release
|
|
|
|
/* ============================================
|
|
I2Cdev device library code is placed under the MIT license
|
|
Copyright (c) 2012 Jeff Rowberg
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to deal
|
|
in the Software without restriction, including without limitation the rights
|
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
copies of the Software, and to permit persons to whom the Software is
|
|
furnished to do so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in
|
|
all copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
THE SOFTWARE.
|
|
===============================================
|
|
*/
|
|
|
|
#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_
|
|
#define _MPU6050_9AXIS_MOTIONAPPS41_H_
|
|
|
|
#include "I2Cdev.h"
|
|
#include "helper_3dmath.h"
|
|
|
|
// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
|
|
#define MPU6050_INCLUDE_DMP_MOTIONAPPS41
|
|
|
|
#include "MPU6050.h"
|
|
|
|
// Tom Carpenter's conditional PROGMEM code
|
|
// http://forum.arduino.cc/index.php?topic=129407.0
|
|
#ifndef __arm__
|
|
#include <avr/pgmspace.h>
|
|
#else
|
|
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
|
|
#ifndef __PGMSPACE_H_
|
|
#define __PGMSPACE_H_ 1
|
|
#include <inttypes.h>
|
|
|
|
#define PROGMEM
|
|
#define PGM_P const char *
|
|
#define PSTR(str) (str)
|
|
#define F(x) x
|
|
|
|
typedef void prog_void;
|
|
typedef char prog_char;
|
|
typedef unsigned char prog_uchar;
|
|
typedef int8_t prog_int8_t;
|
|
typedef uint8_t prog_uint8_t;
|
|
typedef int16_t prog_int16_t;
|
|
typedef uint16_t prog_uint16_t;
|
|
typedef int32_t prog_int32_t;
|
|
typedef uint32_t prog_uint32_t;
|
|
|
|
#define strcpy_P(dest, src) strcpy((dest), (src))
|
|
#define strcat_P(dest, src) strcat((dest), (src))
|
|
#define strcmp_P(a, b) strcmp((a), (b))
|
|
|
|
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
|
|
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
|
|
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
|
|
#define pgm_read_float(addr) (*(const float *)(addr))
|
|
|
|
#define pgm_read_byte_near(addr) pgm_read_byte(addr)
|
|
#define pgm_read_word_near(addr) pgm_read_word(addr)
|
|
#define pgm_read_dword_near(addr) pgm_read_dword(addr)
|
|
#define pgm_read_float_near(addr) pgm_read_float(addr)
|
|
#define pgm_read_byte_far(addr) pgm_read_byte(addr)
|
|
#define pgm_read_word_far(addr) pgm_read_word(addr)
|
|
#define pgm_read_dword_far(addr) pgm_read_dword(addr)
|
|
#define pgm_read_float_far(addr) pgm_read_float(addr)
|
|
#endif
|
|
#endif
|
|
|
|
// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
|
|
// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
|
|
// after moving string constants to flash memory storage using the F()
|
|
// compiler macro (Arduino IDE 1.0+ required).
|
|
|
|
//#define DEBUG
|
|
#ifdef DEBUG
|
|
#define DEBUG_PRINT(x) Serial.print(x)
|
|
#define DEBUG_PRINTF(x, y) Serial.print(x, y)
|
|
#define DEBUG_PRINTLN(x) Serial.println(x)
|
|
#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
|
|
#else
|
|
#define DEBUG_PRINT(x)
|
|
#define DEBUG_PRINTF(x, y)
|
|
#define DEBUG_PRINTLN(x)
|
|
#define DEBUG_PRINTLNF(x, y)
|
|
#endif
|
|
|
|
#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[]
|
|
#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[]
|
|
#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[]
|
|
|
|
/* ================================================================================================ *
|
|
| Default MotionApps v4.1 48-byte FIFO packet structure: |
|
|
| |
|
|
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
|
|
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
|
|
| |
|
|
| [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
|
|
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
|
|
* ================================================================================================ */
|
|
|
|
// this block of memory gets written to the MPU on start-up, and it seems
|
|
// to be volatile memory, so it has to be done each time (it only takes ~1
|
|
// second though)
|
|
const prog_uchar dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
|
|
// bank 0, 256 bytes
|
|
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
|
|
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
|
|
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
|
|
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
|
|
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
|
|
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
|
|
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
|
|
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
|
|
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
|
|
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
|
|
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
|
|
|
|
// bank 1, 256 bytes
|
|
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
|
|
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
|
|
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
|
|
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
|
|
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
|
|
|
|
// bank 2, 256 bytes
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
|
|
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
|
|
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
|
|
// bank 3, 256 bytes
|
|
0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
|
|
0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
|
|
0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
|
|
0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
|
|
0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
|
|
0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
|
|
0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
|
|
0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
|
|
0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
|
|
0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
|
|
0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
|
|
0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
|
|
0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
|
|
0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
|
|
0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
|
|
0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,
|
|
|
|
// bank 4, 256 bytes
|
|
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
|
|
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
|
|
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
|
|
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
|
|
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
|
|
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
|
|
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
|
|
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
|
|
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
|
|
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
|
|
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
|
|
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
|
|
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
|
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
|
|
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
|
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
|
|
|
|
// bank 5, 256 bytes
|
|
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
|
|
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
|
|
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
|
|
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
|
|
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
|
|
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
|
|
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
|
|
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
|
|
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
|
|
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
|
|
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
|
|
0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
|
|
0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
|
|
0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
|
|
0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
|
|
0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,
|
|
|
|
// bank 6, 256 bytes
|
|
0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
|
|
0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
|
|
0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
|
|
0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
|
|
0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
|
|
0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
|
|
0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
|
|
0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
|
|
0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
|
|
0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
|
|
0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
|
|
0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
|
|
0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
|
|
0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
|
|
0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
|
|
0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,
|
|
|
|
// bank 7, 170 bytes (remainder)
|
|
0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
|
|
0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
|
|
0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
|
|
0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
|
|
0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
|
|
0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
|
|
0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
|
|
0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
|
|
0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
|
|
0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
|
|
0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
|
|
};
|
|
|
|
const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
|
|
// BANK OFFSET LENGTH [DATA]
|
|
0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ?
|
|
0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
|
|
0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
|
|
0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration
|
|
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
|
|
0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration
|
|
0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration
|
|
0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration
|
|
|
|
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
|
|
0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01
|
|
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
|
|
0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10
|
|
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
|
|
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
|
|
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
|
|
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
|
|
0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22
|
|
|
|
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
|
|
0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
|
|
0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
|
|
0x00, 0xA3, 0x01, 0x00, // ?
|
|
0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
|
|
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
|
|
0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer
|
|
0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
|
|
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
|
|
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
|
|
0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ?
|
|
0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
|
|
// SPECIAL 0x01 = enable interrupts
|
|
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
|
|
0x07, 0xA7, 0x01, 0xFE, // ?
|
|
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
|
|
0x07, 0x67, 0x01, 0x9A, // ?
|
|
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
|
|
0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
|
|
0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate
|
|
|
|
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
|
|
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
|
|
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
|
|
|
|
// It is important to make sure the host processor can keep up with reading and processing
|
|
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
|
|
};
|
|
|
|
const prog_uchar dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
|
|
0x01, 0xB2, 0x02, 0xFF, 0xF5,
|
|
0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0,
|
|
0x00, 0xA3, 0x01, 0x00,
|
|
0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D,
|
|
0x01, 0x6A, 0x02, 0x06, 0x00,
|
|
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
|
|
0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
|
0x01, 0x08, 0x02, 0x01, 0x20,
|
|
0x01, 0x0A, 0x02, 0x00, 0x4E,
|
|
0x01, 0x02, 0x02, 0xFE, 0xB3,
|
|
0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ
|
|
0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00,
|
|
0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
|
|
0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00,
|
|
0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00,
|
|
0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00,
|
|
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
|
|
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
|
|
};
|
|
|
|
uint8_t MPU6050::dmpInitialize() {
|
|
// reset device
|
|
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
|
|
reset();
|
|
delay(30); // wait after reset
|
|
|
|
// disable sleep mode
|
|
DEBUG_PRINTLN(F("Disabling sleep mode..."));
|
|
setSleepEnabled(false);
|
|
|
|
// get MPU product ID
|
|
DEBUG_PRINTLN(F("Getting product ID..."));
|
|
//uint8_t productID = 0; //getProductID();
|
|
DEBUG_PRINT(F("Product ID = "));
|
|
DEBUG_PRINT(productID);
|
|
|
|
// get MPU hardware revision
|
|
DEBUG_PRINTLN(F("Selecting user bank 16..."));
|
|
setMemoryBank(0x10, true, true);
|
|
DEBUG_PRINTLN(F("Selecting memory byte 6..."));
|
|
setMemoryStartAddress(0x06);
|
|
DEBUG_PRINTLN(F("Checking hardware revision..."));
|
|
uint8_t hwRevision = readMemoryByte();
|
|
DEBUG_PRINT(F("Revision @ user[16][6] = "));
|
|
DEBUG_PRINTLNF(hwRevision, HEX);
|
|
DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
|
|
setMemoryBank(0, false, false);
|
|
|
|
// check OTP bank valid
|
|
DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
|
|
uint8_t otpValid = getOTPBankValid();
|
|
DEBUG_PRINT(F("OTP bank is "));
|
|
DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
|
|
|
|
// get X/Y/Z gyro offsets
|
|
DEBUG_PRINTLN(F("Reading gyro offset values..."));
|
|
int8_t xgOffset = getXGyroOffset();
|
|
int8_t ygOffset = getYGyroOffset();
|
|
int8_t zgOffset = getZGyroOffset();
|
|
DEBUG_PRINT(F("X gyro offset = "));
|
|
DEBUG_PRINTLN(xgOffset);
|
|
DEBUG_PRINT(F("Y gyro offset = "));
|
|
DEBUG_PRINTLN(ygOffset);
|
|
DEBUG_PRINT(F("Z gyro offset = "));
|
|
DEBUG_PRINTLN(zgOffset);
|
|
|
|
I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ?
|
|
|
|
DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled"));
|
|
I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32);
|
|
|
|
// enable MPU AUX I2C bypass mode
|
|
//DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode..."));
|
|
//setI2CBypassEnabled(true);
|
|
|
|
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
|
|
//mag -> setMode(0);
|
|
I2Cdev::writeByte(0x0E, 0x0A, 0x00);
|
|
|
|
DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
|
|
//mag -> setMode(0x0F);
|
|
I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
|
|
|
|
DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
|
|
int8_t asax, asay, asaz;
|
|
//mag -> getAdjustment(&asax, &asay, &asaz);
|
|
I2Cdev::readBytes(0x0E, 0x10, 3, buffer);
|
|
asax = (int8_t)buffer[0];
|
|
asay = (int8_t)buffer[1];
|
|
asaz = (int8_t)buffer[2];
|
|
DEBUG_PRINT(F("Adjustment X/Y/Z = "));
|
|
DEBUG_PRINT(asax);
|
|
DEBUG_PRINT(F(" / "));
|
|
DEBUG_PRINT(asay);
|
|
DEBUG_PRINT(F(" / "));
|
|
DEBUG_PRINTLN(asaz);
|
|
|
|
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
|
|
//mag -> setMode(0);
|
|
I2Cdev::writeByte(0x0E, 0x0A, 0x00);
|
|
|
|
// load DMP code into memory banks
|
|
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
|
|
DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
|
|
DEBUG_PRINTLN(F(" bytes)"));
|
|
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
|
|
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
|
|
|
|
DEBUG_PRINTLN(F("Configuring DMP and related settings..."));
|
|
|
|
// write DMP configuration
|
|
DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
|
|
DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
|
|
DEBUG_PRINTLN(F(" bytes in config def)"));
|
|
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
|
|
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
|
|
|
|
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
|
|
setIntEnabled(0x12);
|
|
|
|
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
|
|
setRate(4); // 1khz / (1 + 4) = 200 Hz
|
|
|
|
DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
|
|
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
|
|
|
|
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
|
|
setDLPFMode(MPU6050_DLPF_BW_42);
|
|
|
|
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
|
|
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
|
|
|
|
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
|
|
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
|
|
|
|
DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
|
|
setDMPConfig1(0x03);
|
|
setDMPConfig2(0x00);
|
|
|
|
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
|
|
setOTPBankValid(false);
|
|
|
|
DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values..."));
|
|
setXGyroOffset(xgOffset);
|
|
setYGyroOffset(ygOffset);
|
|
setZGyroOffset(zgOffset);
|
|
|
|
DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
|
|
setXGyroOffsetUser(0);
|
|
setYGyroOffsetUser(0);
|
|
setZGyroOffsetUser(0);
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)..."));
|
|
uint8_t dmpUpdate[16], j;
|
|
uint16_t pos = 0;
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
|
resetFIFO();
|
|
|
|
DEBUG_PRINTLN(F("Reading FIFO count..."));
|
|
uint8_t fifoCount = getFIFOCount();
|
|
|
|
DEBUG_PRINT(F("Current FIFO count="));
|
|
DEBUG_PRINTLN(fifoCount);
|
|
uint8_t fifoBuffer[128];
|
|
//getFIFOBytes(fifoBuffer, fifoCount);
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Disabling all standby flags..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00);
|
|
|
|
DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00);
|
|
|
|
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
|
|
setMotionDetectionThreshold(2);
|
|
|
|
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
|
|
setZeroMotionDetectionThreshold(156);
|
|
|
|
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
|
|
setMotionDetectionDuration(80);
|
|
|
|
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
|
|
setZeroMotionDetectionDuration(0);
|
|
|
|
DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
|
|
//mag -> setMode(1);
|
|
I2Cdev::writeByte(0x0E, 0x0A, 0x01);
|
|
|
|
// setup AK8975 (0x0E) as Slave 0 in read mode
|
|
DEBUG_PRINTLN(F("Setting up AK8975 read slave 0..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);
|
|
|
|
// setup AK8975 (0x0E) as Slave 2 in write mode
|
|
DEBUG_PRINTLN(F("Setting up AK8975 write slave 2..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01);
|
|
|
|
// setup I2C timing/delay control
|
|
DEBUG_PRINTLN(F("Setting up slave access delay..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
|
|
|
|
// enable interrupts
|
|
DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00);
|
|
|
|
// enable I2C master mode and reset DMP/FIFO
|
|
DEBUG_PRINTLN(F("Enabling I2C master mode..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
|
|
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24);
|
|
DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
|
|
DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
|
|
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8);
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
#ifdef DEBUG
|
|
DEBUG_PRINT(F("Read bytes: "));
|
|
for (j = 0; j < 4; j++) {
|
|
DEBUG_PRINTF(dmpUpdate[3 + j], HEX);
|
|
DEBUG_PRINT(" ");
|
|
}
|
|
DEBUG_PRINTLN("");
|
|
#endif
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Waiting for FIRO count >= 46..."));
|
|
while ((fifoCount = getFIFOCount()) < 46);
|
|
DEBUG_PRINTLN(F("Reading FIFO..."));
|
|
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
|
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
|
getIntStatus();
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
|
|
while ((fifoCount = getFIFOCount()) < 48);
|
|
DEBUG_PRINTLN(F("Reading FIFO..."));
|
|
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
|
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
|
getIntStatus();
|
|
DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
|
|
while ((fifoCount = getFIFOCount()) < 48);
|
|
DEBUG_PRINTLN(F("Reading FIFO..."));
|
|
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
|
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
|
getIntStatus();
|
|
|
|
DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)..."));
|
|
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
|
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
|
|
|
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
|
|
setDMPEnabled(false);
|
|
|
|
DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer..."));
|
|
dmpPacketSize = 48;
|
|
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
|
|
return 3; // TODO: proper error code for no memory
|
|
}*/
|
|
|
|
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
|
|
resetFIFO();
|
|
getIntStatus();
|
|
} else {
|
|
DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
|
|
return 2; // configuration block loading failed
|
|
}
|
|
} else {
|
|
DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
|
|
return 1; // main binary block loading failed
|
|
}
|
|
return 0; // success
|
|
}
|
|
|
|
bool MPU6050::dmpPacketAvailable() {
|
|
return getFIFOCount() >= dmpGetFIFOPacketSize();
|
|
}
|
|
|
|
// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
|
|
// uint8_t MPU6050::dmpGetFIFORate();
|
|
// uint8_t MPU6050::dmpGetSampleStepSizeMS();
|
|
// uint8_t MPU6050::dmpGetSampleFrequency();
|
|
// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
|
|
|
|
//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
|
|
//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
|
|
//uint8_t MPU6050::dmpRunFIFORateProcesses();
|
|
|
|
// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
|
|
|
|
uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]);
|
|
data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]);
|
|
data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]);
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = (packet[34] << 8) + packet[35];
|
|
data[1] = (packet[38] << 8) + packet[39];
|
|
data[2] = (packet[42] << 8) + packet[43];
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
v -> x = (packet[34] << 8) + packet[35];
|
|
v -> y = (packet[38] << 8) + packet[39];
|
|
v -> z = (packet[42] << 8) + packet[43];
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
|
|
data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
|
|
data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
|
|
data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = ((packet[0] << 8) + packet[1]);
|
|
data[1] = ((packet[4] << 8) + packet[5]);
|
|
data[2] = ((packet[8] << 8) + packet[9]);
|
|
data[3] = ((packet[12] << 8) + packet[13]);
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
int16_t qI[4];
|
|
uint8_t status = dmpGetQuaternion(qI, packet);
|
|
if (status == 0) {
|
|
q -> w = (float)qI[0] / 16384.0f;
|
|
q -> x = (float)qI[1] / 16384.0f;
|
|
q -> y = (float)qI[2] / 16384.0f;
|
|
q -> z = (float)qI[3] / 16384.0f;
|
|
return 0;
|
|
}
|
|
return status; // int16 return value, indicates error if this line is reached
|
|
}
|
|
// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
|
|
uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
|
|
data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
|
|
data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = (packet[16] << 8) + packet[17];
|
|
data[1] = (packet[20] << 8) + packet[21];
|
|
data[2] = (packet[24] << 8) + packet[25];
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) {
|
|
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
|
if (packet == 0) packet = dmpPacketBuffer;
|
|
data[0] = (packet[28] << 8) + packet[29];
|
|
data[1] = (packet[30] << 8) + packet[31];
|
|
data[2] = (packet[32] << 8) + packet[33];
|
|
return 0;
|
|
}
|
|
// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
|
|
// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
|
|
uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
|
|
// get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
|
|
v -> x = vRaw -> x - gravity -> x*4096;
|
|
v -> y = vRaw -> y - gravity -> y*4096;
|
|
v -> z = vRaw -> z - gravity -> z*4096;
|
|
return 0;
|
|
}
|
|
// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
|
|
uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
|
|
// rotate measured 3D acceleration vector into original state
|
|
// frame of reference based on orientation quaternion
|
|
memcpy(v, vReal, sizeof(VectorInt16));
|
|
v -> rotate(q);
|
|
return 0;
|
|
}
|
|
// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
|
|
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
|
|
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
|
|
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
|
|
v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
|
|
return 0;
|
|
}
|
|
// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
|
|
|
|
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
|
|
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
|
|
data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
|
|
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
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
|
|
// yaw: (about Z axis)
|
|
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
|
|
// pitch: (nose up/down, about Y axis)
|
|
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
|
|
// roll: (tilt left/right, about X axis)
|
|
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
|
|
return 0;
|
|
}
|
|
|
|
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
|
|
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
|
|
|
|
uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
|
|
/*for (uint8_t k = 0; k < dmpPacketSize; k++) {
|
|
if (dmpData[k] < 0x10) Serial.print("0");
|
|
Serial.print(dmpData[k], HEX);
|
|
Serial.print(" ");
|
|
}
|
|
Serial.print("\n");*/
|
|
//Serial.println((uint16_t)dmpPacketBuffer);
|
|
return 0;
|
|
}
|
|
uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
|
|
uint8_t status;
|
|
uint8_t buf[dmpPacketSize];
|
|
for (uint8_t i = 0; i < numPackets; i++) {
|
|
// read packet from FIFO
|
|
getFIFOBytes(buf, dmpPacketSize);
|
|
|
|
// process packet
|
|
if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
|
|
|
|
// increment external process count variable, if supplied
|
|
if (processed != 0) *processed++;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
|
|
|
|
// uint8_t MPU6050::dmpInitFIFOParam();
|
|
// uint8_t MPU6050::dmpCloseFIFO();
|
|
// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
|
|
// uint8_t MPU6050::dmpDecodeQuantizedAccel();
|
|
// uint32_t MPU6050::dmpGetGyroSumOfSquare();
|
|
// uint32_t MPU6050::dmpGetAccelSumOfSquare();
|
|
// void MPU6050::dmpOverrideQuaternion(long *q);
|
|
uint16_t MPU6050::dmpGetFIFOPacketSize() {
|
|
return dmpPacketSize;
|
|
}
|
|
|
|
#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */
|