1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
Diff: MPU6050_6Axis_MotionApps20.h
- Revision:
- 7:d5845b617139
- Parent:
- 6:f38dfe62d74c
- Child:
- 8:4ee054567b6c
diff -r f38dfe62d74c -r d5845b617139 MPU6050_6Axis_MotionApps20.h --- a/MPU6050_6Axis_MotionApps20.h Sat Jan 30 17:12:45 2016 +0000 +++ b/MPU6050_6Axis_MotionApps20.h Sun Jan 31 09:12:19 2016 +0000 @@ -1,35 +1,3 @@ -// 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_ @@ -43,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 @@ -53,7 +21,7 @@ #define PROGMEM #define PGM_P const char * - // #define PSTR(str) (str) + #define PSTR(str) (str) #define F(x) x typedef void prog_void; @@ -65,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,34 +70,12 @@ #define DEBUG #ifdef DEBUG - Serial debug_serial(USBTX, USBRX); - template <typename T> - void inline DEBUG_PRINT(T x) { debug_serial.printf("%d", x); } - void inline DEBUG_PRINT(const char* x) { debug_serial.printf(x); } - - template <typename T> - void inline DEBUG_PRINTLN(T x) { DEBUG_PRINT(x);debug_serial.printf("\r\n"); } - - enum Format { BIN, OCT, DEC, HEX }; - - template <typename T> - void inline DEBUG_PRINTF(T x, Format fmt) { - if(fmt == OCT) { - debug_serial.printf("%o", (x)); - } else if (fmt == DEC) { - debug_serial.printf("%d", (x)); - } else if (fmt == HEX) { - debug_serial.printf("%x", (x)); - } else { - debug_serial.printf("We aren't supporting this format: %d", (x)); - } - } - - template <typename T> - void inline DEBUG_PRINTLNF(T x, Format fmt) { - DEBUG_PRINTF(x, fmt); - debug_serial.printf("\r\n"); - } + #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) @@ -368,17 +314,15 @@ 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...")); @@ -518,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]); @@ -537,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]); @@ -601,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) { @@ -658,25 +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]; + 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); @@ -750,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; }