1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
Diff: MPU6050_6Axis_MotionApps20.h
- Revision:
- 6:f38dfe62d74c
- Parent:
- 5:7d1bf3ce0053
- Child:
- 7:d5845b617139
diff -r 7d1bf3ce0053 -r f38dfe62d74c MPU6050_6Axis_MotionApps20.h --- a/MPU6050_6Axis_MotionApps20.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050_6Axis_MotionApps20.h Sat Jan 30 17:12:45 2016 +0000 @@ -33,8 +33,6 @@ #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ -#include <iostream> - #include "I2Cdev.h" #include "helper_3dmath.h" @@ -55,7 +53,7 @@ #define PROGMEM #define PGM_P const char * - #define PSTR(str) (str) + // #define PSTR(str) (str) #define F(x) x typedef void prog_void; @@ -102,13 +100,36 @@ // 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 + 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"); + } #else #define DEBUG_PRINT(x) #define DEBUG_PRINTF(x, y) @@ -329,8 +350,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...")); @@ -358,19 +378,19 @@ 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(otpValid ? 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 +401,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 +443,9 @@ setOTPBankValid(false); DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); - //setXGyroOffsetTC(xgOffsetTC); - //setYGyroOffsetTC(ygOffsetTC); - //setZGyroOffsetTC(zgOffsetTC); + setXGyroOffsetTC(xgOffsetTC); + setYGyroOffsetTC(ygOffsetTC); + setZGyroOffsetTC(zgOffsetTC); //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); //setXGyroOffset(0); @@ -652,6 +671,14 @@ 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); // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {