Diff: MPU6050_6Axis_MotionApps20.h

Revision:

6:f38dfe62d74c

Parent:

5:7d1bf3ce0053

Child:

7:d5845b617139

--- 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) {