Weber Yang
For nucleoF103 change sda,scl to PB_7,PB_6
Dependents: AGV_0411 AGV_SMT_Sevro AGV_SMT
Fork of
MPU6050-DMP
by 
takaaki mastuzawa
Diff: MPU6050_6Axis_MotionApps20.h
- Revision:
- 8:473800bd51ac
- Parent:
- 6:fe57b4405983
--- a/MPU6050_6Axis_MotionApps20.h Thu Apr 12 01:11:08 2018 +0000
+++ b/MPU6050_6Axis_MotionApps20.h Thu May 10 08:15:44 2018 +0000
@@ -1,40 +1,6 @@
-// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
-// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
-// 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// 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_6AXIS_MOTIONAPPS20_H_
#define _MPU6050_6AXIS_MOTIONAPPS20_H_
-//#include <iostream>
-
#include "I2Cdev.h"
#include "helper_3dmath.h"
@@ -45,7 +11,7 @@
// Tom Carpenter's conditional PROGMEM code
// http://forum.arduino.cc/index.php?topic=129407.0
-#ifndef __arm__
+#ifdef __AVR__
#include <avr/pgmspace.h>
#else
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
@@ -67,16 +33,16 @@
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)
@@ -102,13 +68,14 @@
// after moving string constants to flash memory storage using the F()
// compiler macro (Arduino IDE 1.0+ required).
-//#define DEBUG
+// #define DEBUG
#ifdef DEBUG
- #define DEBUG_PRINT(x) std::cout << x //Serial.print(x)
- #define DEBUG_PRINTF(x, y) std::cout << x //Serial.print(x, y)
- #define DEBUG_PRINTLN(x) std::cout << x << std::endl //Serial.println(x)
- #define DEBUG_PRINTLNF(x, y) std::cout << x << std::endl //Serial.println(x, y)
- #define F(x) x
+ #include "ArduinoSerial.h"
+ ArduinoSerial arduinoSerial;
+ #define DEBUG_PRINT(x) arduinoSerial.print(x)
+ #define DEBUG_PRINTF(x, y) arduinoSerial.print(x, y)
+ #define DEBUG_PRINTLN(x) arduinoSerial.println(x)
+ #define DEBUG_PRINTLNF(x, y) arduinoSerial.println(x, y)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTF(x, y)
@@ -305,7 +272,7 @@
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
- 0x02, 0x16, 0x02, 0x00, 0x00 // D_0_22 inv_set_fifo_rate
+ 0x02, 0x16, 0x02, 0x00, 0x01 // 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.
@@ -329,8 +296,7 @@
// reset device
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
reset();
- //delay(30); // wait after reset
- wait_ms(30);
+ wait_ms(30); // wait after reset
// enable sleep mode and wake cycle
/*Serial.println(F("Enabling sleep mode..."));
@@ -348,29 +314,27 @@
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_PRINTLNF(readMemoryByte(), 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!")));
+ DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!"));
// get X/Y/Z gyro offsets
DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
- //int8_t xgOffsetTC = getXGyroOffsetTC();
- //int8_t ygOffsetTC = getYGyroOffsetTC();
- //int8_t zgOffsetTC = getZGyroOffsetTC();
- //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);
+ int8_t xgOffsetTC = getXGyroOffsetTC();
+ int8_t ygOffsetTC = getYGyroOffsetTC();
+ int8_t zgOffsetTC = getZGyroOffsetTC();
+ DEBUG_PRINT(F("X gyro offset = "));
+ DEBUG_PRINTLN(xgOffsetTC);
+ DEBUG_PRINT(F("Y gyro offset = "));
+ DEBUG_PRINTLN(ygOffsetTC);
+ DEBUG_PRINT(F("Z gyro offset = "));
+ DEBUG_PRINTLN(zgOffsetTC);
// setup weird slave stuff (?)
DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
@@ -381,7 +345,6 @@
setSlaveAddress(0, 0x68);
DEBUG_PRINTLN(F("Resetting I2C Master control..."));
resetI2CMaster();
- //delay(20);
wait_ms(20);
// load DMP code into memory banks
@@ -424,9 +387,9 @@
setOTPBankValid(false);
DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
- //setXGyroOffsetTC(xgOffsetTC);
- //setYGyroOffsetTC(ygOffsetTC);
- //setZGyroOffsetTC(zgOffsetTC);
+ setXGyroOffset(xgOffsetTC);
+ setYGyroOffset(ygOffsetTC);
+ setZGyroOffset(zgOffsetTC);
//DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
//setXGyroOffset(0);
@@ -499,10 +462,9 @@
getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN(F("Reading interrupt status..."));
- uint8_t mpuIntStatus = getIntStatus();
DEBUG_PRINT(F("Current interrupt status="));
- DEBUG_PRINTLNF(mpuIntStatus, HEX);
+ DEBUG_PRINTLNF(getIntStatus(), HEX);
DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
@@ -518,10 +480,9 @@
getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN(F("Reading interrupt status..."));
- mpuIntStatus = getIntStatus();
DEBUG_PRINT(F("Current interrupt status="));
- DEBUG_PRINTLNF(mpuIntStatus, HEX);
+ DEBUG_PRINTLNF(getIntStatus(), HEX);
DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
@@ -582,43 +543,43 @@
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[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]);
- data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]);
- data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]);
+ data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]);
+ data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]);
+ data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]);
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[28] << 8) + packet[29];
- data[1] = (packet[32] << 8) + packet[33];
- data[2] = (packet[36] << 8) + packet[37];
+ data[0] = (packet[28] << 8) | packet[29];
+ data[1] = (packet[32] << 8) | packet[33];
+ data[2] = (packet[36] << 8) | packet[37];
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[28] << 8) + packet[29];
- v -> y = (packet[32] << 8) + packet[33];
- v -> z = (packet[36] << 8) + packet[37];
+ v -> x = (packet[28] << 8) | packet[29];
+ v -> y = (packet[32] << 8) | packet[33];
+ v -> z = (packet[36] << 8) | packet[37];
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]);
+ data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
+ data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
+ data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
+ data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)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]);
+ 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) {
@@ -639,17 +600,25 @@
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]);
+ data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
+ data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
+ data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)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];
+ 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::dmpGetGyro(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[16] << 8) | packet[17];
+ v -> y = (packet[20] << 8) | packet[21];
+ v -> z = (packet[24] << 8) | packet[25];
return 0;
}
// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
@@ -723,9 +692,9 @@
// process packet
if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
-
+
// increment external process count variable, if supplied
- if (processed != 0) *processed++;
+ if (processed != 0) (*processed)++;
}
return 0;
}