Revision 2:42c4f3a7813f, committed 2016-01-31

Comitter:

syundo0730

Date:

Sun Jan 31 14:12:13 2016 +0000

Parent:

1:ec0a08108442

Commit message:

integrated the example for DMP and raw value

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Examples/MPU6050_DMP6.cpp	Sun Jan 31 14:12:13 2016 +0000
@@ -0,0 +1,307 @@
+#include "MPU6050_DMP6.h"
+
+// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
+// for both classes must be in the include path of your project
+#include "I2Cdev.h"
+
+#include "MPU6050_6Axis_MotionApps20.h"
+//#include "MPU6050.h" // not necessary if using MotionApps include file
+
+#include "ArduinoSerial.h"
+
+/* =========================================================================
+NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
+depends on the MPU-6050's INT pin being connected to the Arduino's
+external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
+digital I/O pin 2.
+* ========================================================================= */
+
+// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
+// quaternion components in a [w, x, y, z] format (not best for parsing
+// on a remote host such as Processing or something though)
+//#define OUTPUT_READABLE_QUATERNION
+
+// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
+// (in degrees) calculated from the quaternions coming from the FIFO.
+// Note that Euler angles suffer from gimbal lock (for more info, see
+// http://en.wikipedia.org/wiki/Gimbal_lock)
+//#define OUTPUT_READABLE_EULER
+
+// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
+// pitch/roll angles (in degrees) calculated from the quaternions coming
+// from the FIFO. Note this also requires gravity vector calculations.
+// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
+// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
+#define OUTPUT_READABLE_YAWPITCHROLL
+
+// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
+// components with gravity removed. This acceleration reference frame is
+// not compensated for orientation, so +X is always +X according to the
+// sensor, just without the effects of gravity. If you want acceleration
+// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
+//#define OUTPUT_READABLE_REALACCEL
+
+// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
+// components with gravity removed and adjusted for the world frame of
+// reference (yaw is relative to initial orientation, since no magnetometer
+// is present in this case). Could be quite handy in some cases.
+//#define OUTPUT_READABLE_WORLDACCEL
+
+// uncomment "OUTPUT_TEAPOT" if you want output that matches the
+// format used for the InvenSense teapot demo
+//#define OUTPUT_TEAPOT
+
+#ifndef M_PI
+#define M_PI 3.14159265358979
+#endif
+
+namespace MPU6050DMP6 {
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 mpu;
+//MPU6050 mpu(0x69); // <-- use for AD0 high
+
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+// packet structure for InvenSense teapot demo
+uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
+
+DigitalOut led1(LED1);
+InterruptIn checkpin(p29);
+ArduinoSerial arduinoSerial;
+
+
+// ================================================================
+// ===               INTERRUPT DETECTION ROUTINE                ===
+// ================================================================
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady()
+{
+    mpuInterrupt = true;
+}
+
+
+
+// ================================================================
+// ===                      INITIAL SETUP                       ===
+// ================================================================
+
+void setup()
+{
+    // initialize arduinoSerial communication
+    // (115200 chosen because it is required for Teapot Demo output, but it's
+    // really up to you depending on your project)
+    arduinoSerial.begin(115200);
+
+    // initialize device
+    arduinoSerial.println(F("Initializing I2C devices..."));
+    mpu.initialize();
+
+    // verify connection
+    arduinoSerial.println(F("Testing device connections..."));
+    arduinoSerial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
+
+    // wait for ready
+    // arduinoSerial.println(F("\nSend any character to begin DMP programming and demo: "));
+    // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer
+    // while (!arduinoSerial.available());                 // wait for data
+    // while (arduinoSerial.available() && arduinoSerial.read()); // empty buffer again
+
+    // load and configure the DMP
+    arduinoSerial.println(F("Initializing DMP..."));
+    devStatus = mpu.dmpInitialize();
+
+    // supply your own gyro offsets here, scaled for min sensitivity
+    mpu.setXGyroOffset(220);
+    mpu.setYGyroOffset(76);
+    mpu.setZGyroOffset(-85);
+    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
+
+    // make sure it worked (returns 0 if so)
+    if (devStatus == 0) {
+        // turn on the DMP, now that it's ready
+        arduinoSerial.println(F("Enabling DMP..."));
+        mpu.setDMPEnabled(true);
+
+        // enable Arduino interrupt detection
+        arduinoSerial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
+        checkpin.rise(&dmpDataReady);
+        mpuIntStatus = mpu.getIntStatus();
+
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        arduinoSerial.println(F("DMP ready! Waiting for first interrupt..."));
+        dmpReady = true;
+
+        // get expected DMP packet size for later comparison
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    } else {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        arduinoSerial.print(F("DMP Initialization failed (code "));
+        arduinoSerial.print(devStatus);
+        arduinoSerial.println(F(")"));
+    }
+
+}
+
+
+
+// ================================================================
+// ===                    MAIN PROGRAM LOOP                     ===
+// ================================================================
+
+void loop()
+{
+    // if programming failed, don't try to do anything
+    if (!dmpReady) return;
+
+    // wait for MPU interrupt or extra packet(s) available
+    while (!mpuInterrupt && fifoCount < packetSize) {
+        // other program behavior stuff here
+        // .
+        // .
+        // .
+        // if you are really paranoid you can frequently test in between other
+        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
+        // while() loop to immediately process the MPU data
+        // .
+        // .
+        // .
+    }
+
+    // reset interrupt flag and get INT_STATUS byte
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = mpu.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+        // reset so we can continue cleanly
+        mpu.resetFIFO();
+        arduinoSerial.println(F("FIFO overflow!"));
+
+        // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    } else if (mpuIntStatus & 0x02) {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+        // read a packet from FIFO
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount -= packetSize;
+
+#ifdef OUTPUT_READABLE_QUATERNION
+        // display quaternion values in easy matrix form: w x y z
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        arduinoSerial.print("quat\t");
+        arduinoSerial.print(q.w);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(q.x);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(q.y);
+        arduinoSerial.print("\t");
+        arduinoSerial.println(q.z);
+#endif
+
+#ifdef OUTPUT_READABLE_EULER
+        // display Euler angles in degrees
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetEuler(euler, &q);
+        arduinoSerial.print("euler\t");
+        arduinoSerial.print(euler[0] * 180/M_PI);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(euler[1] * 180/M_PI);
+        arduinoSerial.print("\t");
+        arduinoSerial.println(euler[2] * 180/M_PI);
+#endif
+
+#ifdef OUTPUT_READABLE_YAWPITCHROLL
+        // display Euler angles in degrees
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        arduinoSerial.print("ypr\t");
+        arduinoSerial.print(ypr[0] * 180/M_PI);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(ypr[1] * 180/M_PI);
+        arduinoSerial.print("\t");
+        arduinoSerial.println(ypr[2] * 180/M_PI);
+#endif
+
+#ifdef OUTPUT_READABLE_REALACCEL
+        // display real acceleration, adjusted to remove gravity
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetAccel(&aa, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+        arduinoSerial.print("areal\t");
+        arduinoSerial.print(aaReal.x);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(aaReal.y);
+        arduinoSerial.print("\t");
+        arduinoSerial.println(aaReal.z);
+#endif
+
+#ifdef OUTPUT_READABLE_WORLDACCEL 
+        // display initial world-frame acceleration, adjusted to remove gravity
+        // and rotated based on known orientation from quaternion
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetAccel(&aa, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+        mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+        arduinoSerial.print("aworld\t");
+        arduinoSerial.print(aaWorld.x);
+        arduinoSerial.print("\t");
+        arduinoSerial.print(aaWorld.y);
+        arduinoSerial.print("\t");
+        arduinoSerial.println(aaWorld.z);
+#endif
+
+#ifdef OUTPUT_TEAPOT
+        // display quaternion values in InvenSense Teapot demo format:
+        teapotPacket[2] = fifoBuffer[0];
+        teapotPacket[3] = fifoBuffer[1];
+        teapotPacket[4] = fifoBuffer[4];
+        teapotPacket[5] = fifoBuffer[5];
+        teapotPacket[6] = fifoBuffer[8];
+        teapotPacket[7] = fifoBuffer[9];
+        teapotPacket[8] = fifoBuffer[12];
+        teapotPacket[9] = fifoBuffer[13];
+        arduinoSerial.write(teapotPacket, 14);
+        teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
+#endif
+
+        // blink LED to indicate activity
+        if( led1 == 0 ) led1 = 0;
+        else led1 = 1;
+    }
+}
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Examples/MPU6050_DMP6.h	Sun Jan 31 14:12:13 2016 +0000
@@ -0,0 +1,9 @@
+#ifndef _MPU6050_DMP_6_H_
+#define _MPU6050_DMP_6_H_
+
+namespace MPU6050DMP6 {
+    void setup();
+    void loop();
+};
+
+#endif /* _MPU6050_DMP_6_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Examples/MPU6050_raw.cpp	Sun Jan 31 14:12:13 2016 +0000
@@ -0,0 +1,106 @@
+#include "MPU6050_raw.h"
+
+// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
+// for both classes must be in the include path of your project
+#include "I2Cdev.h"
+#include "MPU6050.h"
+
+#include "ArduinoSerial.h"
+
+// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
+// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
+// not so easy to parse, and slow(er) over UART.
+#define OUTPUT_READABLE_ACCELGYRO
+
+// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
+// binary, one right after the other. This is very fast (as fast as possible
+// without compression or data loss), and easy to parse, but impossible to read
+// for a human.
+//#define OUTPUT_BINARY_ACCELGYRO
+
+namespace MPU6050raw {
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 accelgyro;
+//MPU6050 accelgyro(0x69); // <-- use for AD0 high
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+DigitalOut led1(LED1);
+ArduinoSerial arduinoSerial;
+
+void setup() {
+    // initialize arduinoSerial communication
+    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
+    // it's really up to you depending on your project)
+    arduinoSerial.begin(38400);
+
+    // initialize device
+    arduinoSerial.println("Initializing I2C devices...");
+    accelgyro.initialize();
+
+    // verify connection
+    arduinoSerial.println("Testing device connections...");
+    arduinoSerial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
+    // use the code below to change accel/gyro offset values
+    /*
+    arduinoSerial.println("Updating internal sensor offsets...");
+    // -76  -2359   1688    0   0   0
+    arduinoSerial.print(accelgyro.getXAccelOffset()); arduinoSerial.print("\t"); // -76
+    arduinoSerial.print(accelgyro.getYAccelOffset()); arduinoSerial.print("\t"); // -2359
+    arduinoSerial.print(accelgyro.getZAccelOffset()); arduinoSerial.print("\t"); // 1688
+    arduinoSerial.print(accelgyro.getXGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print(accelgyro.getYGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print(accelgyro.getZGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print("\n");
+    accelgyro.setXGyroOffset(220);
+    accelgyro.setYGyroOffset(76);
+    accelgyro.setZGyroOffset(-85);
+    arduinoSerial.print(accelgyro.getXAccelOffset()); arduinoSerial.print("\t"); // -76
+    arduinoSerial.print(accelgyro.getYAccelOffset()); arduinoSerial.print("\t"); // -2359
+    arduinoSerial.print(accelgyro.getZAccelOffset()); arduinoSerial.print("\t"); // 1688
+    arduinoSerial.print(accelgyro.getXGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print(accelgyro.getYGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print(accelgyro.getZGyroOffset()); arduinoSerial.print("\t"); // 0
+    arduinoSerial.print("\n");
+    */
+}
+
+void loop() {
+    // read raw accel/gyro measurements from device
+    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+    // these methods (and a few others) are also available
+    //accelgyro.getAcceleration(&ax, &ay, &az);
+    //accelgyro.getRotation(&gx, &gy, &gz);
+
+    #ifdef OUTPUT_READABLE_ACCELGYRO
+        // display tab-separated accel/gyro x/y/z values
+        arduinoSerial.print("a/g:\t");
+        arduinoSerial.print(ax); arduinoSerial.print("\t");
+        arduinoSerial.print(ay); arduinoSerial.print("\t");
+        arduinoSerial.print(az); arduinoSerial.print("\t");
+        arduinoSerial.print(gx); arduinoSerial.print("\t");
+        arduinoSerial.print(gy); arduinoSerial.print("\t");
+        arduinoSerial.println(gz);
+    #endif
+
+    #ifdef OUTPUT_BINARY_ACCELGYRO
+        arduinoSerial.write((uint8_t)(ax >> 8)); arduinoSerial.write((uint8_t)(ax & 0xFF));
+        arduinoSerial.write((uint8_t)(ay >> 8)); arduinoSerial.write((uint8_t)(ay & 0xFF));
+        arduinoSerial.write((uint8_t)(az >> 8)); arduinoSerial.write((uint8_t)(az & 0xFF));
+        arduinoSerial.write((uint8_t)(gx >> 8)); arduinoSerial.write((uint8_t)(gx & 0xFF));
+        arduinoSerial.write((uint8_t)(gy >> 8)); arduinoSerial.write((uint8_t)(gy & 0xFF));
+        arduinoSerial.write((uint8_t)(gz >> 8)); arduinoSerial.write((uint8_t)(gz & 0xFF));
+    #endif
+
+    // blink LED to indicate activity
+    if( led1 == 0 ) led1 = 0;
+    else led1 = 1;
+}
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Examples/MPU6050_raw.h	Sun Jan 31 14:12:13 2016 +0000
@@ -0,0 +1,9 @@
+#ifndef _MPU6050_RAW_H_
+#define _MPU6050_RAW_H_
+
+namespace MPU6050raw {
+    void setup();
+    void loop();
+};
+
+#endif /* _MPU6050_RAW_H_ */
\ No newline at end of file

--- a/MPU6050-DMP.lib	Sat Nov 23 16:47:59 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://mbed.org/users/syundo0730/code/MPU6050-DMP/#7d1bf3ce0053

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Sun Jan 31 14:12:13 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/syundo0730/code/MPU6050/#4ee054567b6c

--- a/main.cpp	Sat Nov 23 16:47:59 2013 +0000
+++ b/main.cpp	Sun Jan 31 14:12:13 2016 +0000
@@ -1,343 +1,17 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//#include "MPU6050_DMP6.h"
 //
-// Changelog:
-//     2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-//     2012-06-20 - improved FIFO overflow handling and simplified read process
-//     2012-06-19 - completely rearranged DMP initialization code and simplification
-//     2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-//     2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-//     2012-06-05 - add gravity-compensated initial reference frame acceleration output
-//                - add 3D math helper file to DMP6 example sketch
-//                - add Euler output and Yaw/Pitch/Roll output formats
-//     2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-//     2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-//     2012-05-30 - basic DMP initialization working
-
-/* ============================================
-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.
-===============================================
-*/
-
-// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
-// is used in I2Cdev.h
-//#include "Wire.h"
-
-// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
-// for both classes must be in the include path of your project
-#include "I2Cdev.h"
-
-#include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-// class default I2C address is 0x68
-// specific I2C addresses may be passed as a parameter here
-// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
-// AD0 high = 0x69
-
-MPU6050 mpu;
-
-/* =========================================================================
-   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
-   depends on the MPU-6050's INT pin being connected to the Arduino's
-   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
-   digital I/O pin 2.
- * ========================================================================= */
-
-/* =========================================================================
-   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
-   when using Serial.write(buf, len). The Teapot output uses this method.
-   The solution requires a modification to the Arduino USBAPI.h file, which
-   is fortunately simple, but annoying. This will be fixed in the next IDE
-   release. For more info, see these links:
+//int main() {
+//    MPU6050DMP6::setup();
+//    while(1) {
+//        MPU6050DMP6::loop();
+//    }
+//}
 
-   http://arduino.cc/forum/index.php/topic,109987.0.html
-   http://code.google.com/p/arduino/issues/detail?id=958
- * ========================================================================= */
-
-const float M_PI = 3.14159265;
-
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
-// quaternion components in a [w, x, y, z] format (not best for parsing
-// on a remote host such as Processing or something though)
-//#define OUTPUT_READABLE_QUATERNION
-
-// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
-// (in degrees) calculated from the quaternions coming from the FIFO.
-// Note that Euler angles suffer from gimbal lock (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_EULER
-
-// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
-// pitch/roll angles (in degrees) calculated from the quaternions coming
-// from the FIFO. Note this also requires gravity vector calculations.
-// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
-// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
-
-// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
-// components with gravity removed. This acceleration reference frame is
-// not compensated for orientation, so +X is always +X according to the
-// sensor, just without the effects of gravity. If you want acceleration
-// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
-//#define OUTPUT_READABLE_REALACCEL
-
-// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
-// components with gravity removed and adjusted for the world frame of
-// reference (yaw is relative to initial orientation, since no magnetometer
-// is present in this case). Could be quite handy in some cases.
-//#define OUTPUT_READABLE_WORLDACCEL
-
-// uncomment "OUTPUT_TEAPOT" if you want output that matches the
-// format used for the InvenSense teapot demo
-//#define OUTPUT_TEAPOT
-
-// MPU control/status vars
-bool dmpReady = false;  // set true if DMP init was successful
-uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
-
-// orientation/motion vars
-Quaternion q;           // [w, x, y, z]         quaternion container
-VectorInt16 aa;         // [x, y, z]            accel sensor measurements
-VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
-VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
-VectorFloat gravity;    // [x, y, z]            gravity vector
-float euler[3];         // [psi, theta, phi]    Euler angle container
-float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
-
-// packet structure for InvenSense teapot demo
-uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
-
-DigitalOut led1(LED1);
-InterruptIn checkpin(p29);
-Serial pc(USBTX, USBRX);
-
-// ================================================================
-// ===               INTERRUPT DETECTION ROUTINE                ===
-// ================================================================
-
-volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
-void dmpDataReady() {
-    mpuInterrupt = true;
-}
-
-void setup();
-void loop();
+#include "MPU6050_raw.h"
 
 int main() {
-    setup();
+    MPU6050raw::setup();
     while(1) {
-        loop();
-    }
-}
-
-// ================================================================
-// ===                      INITIAL SETUP                       ===
-// ================================================================
-
-void setup() {
-    // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
-    // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
-    // the baud timing being too misaligned with processor ticks. You must use
-    // 38400 or slower in these cases, or use some kind of external separate
-    // crystal solution for the UART timer.
-
-    // initialize device
-    pc.printf("Initializing I2C devices...\r\n");
-    mpu.initialize();
-
-    // verify connection
-    pc.printf("Testing device connections...\r\n");
-    if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
-    else pc.printf("MPU6050 connection failed\r\n");
-    
-    // wait for ready
-    //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
-    //while (Serial.available() && Serial.read()); // empty buffer
-    //while (!Serial.available());                 // wait for data
-    //while (Serial.available() && Serial.read()); // empty buffer again
-
-    // load and configure the DMP
-    pc.printf("Initializing DMP...\r\n");
-    devStatus = mpu.dmpInitialize();
-    
-    // make sure it worked (returns 0 if so)
-    if (devStatus == 0) {
-        // turn on the DMP, now that it's ready
-        pc.printf("Enabling DMP...\r\n");
-        mpu.setDMPEnabled(true);
-
-        // enable Arduino interrupt detection
-        pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
-        checkpin.rise(&dmpDataReady);
-        
-        mpuIntStatus = mpu.getIntStatus();
-
-        // set our DMP Ready flag so the main loop() function knows it's okay to use it
-        pc.printf("DMP ready! Waiting for first interrupt...\r\n");
-        dmpReady = true;
-
-        // get expected DMP packet size for later comparison
-        packetSize = mpu.dmpGetFIFOPacketSize();
-    } else {
-        // ERROR!
-        // 1 = initial memory load failed
-        // 2 = DMP configuration updates failed
-        // (if it's going to break, usually the code will be 1)
-        
-        pc.printf("DDMP Initialization failed (code ");
-        pc.printf("%d", devStatus);
-        pc.printf(")\r\n");
-    }
-
-}
-
-
-
-// ================================================================
-// ===                    MAIN PROGRAM LOOP                     ===
-// ================================================================
-
-void loop() {
-    // if programming failed, don't try to do anything
-    if (!dmpReady) return;
-
-    // wait for MPU interrupt or extra packet(s) available
-    while (!mpuInterrupt && fifoCount < packetSize) {
-        // other program behavior stuff here
-        // .
-        // .
-        // .
-        // if you are really paranoid you can frequently test in between other
-        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
-        // while() loop to immediately process the MPU data
-        // .
-        // .
-        // .
-    }
-
-    // reset interrupt flag and get INT_STATUS byte
-    mpuInterrupt = false;
-    mpuIntStatus = mpu.getIntStatus();
-
-    // get current FIFO count
-    fifoCount = mpu.getFIFOCount();
-
-    // check for overflow (this should never happen unless our code is too inefficient)
-    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-        // reset so we can continue cleanly
-        mpu.resetFIFO();
-        //Serial.println(F("FIFO overflow!"));
-
-    // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
-        // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
-        // read a packet from FIFO
-        mpu.getFIFOBytes(fifoBuffer, packetSize);
-        
-        // track FIFO count here in case there is > 1 packet available
-        // (this lets us immediately read more without waiting for an interrupt)
-        fifoCount -= packetSize;
-
-        #ifdef OUTPUT_READABLE_QUATERNION
-            // display quaternion values in easy matrix form: w x y z
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            printf("quat\t");
-            printf("%f\t", q.w);
-            printf("%f\t", q.x);
-            printf("%f\t", q.y);
-            printf("%f\t\r\n", q.z);
-        #endif
-
-        #ifdef OUTPUT_READABLE_EULER
-            // display Euler angles in degrees
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetEuler(euler, &q);
-            printf("euler\t");
-            printf("%f\t", euler[0] * 180/M_PI);
-            printf("%f\t", euler[1] * 180/M_PI);
-            printf("%f\t\r\n", euler[2] * 180/M_PI);
-        #endif
-
-        #ifdef OUTPUT_READABLE_YAWPITCHROLL
-            // display Euler angles in degrees
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetGravity(&gravity, &q);
-            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-            printf("ypr\t");
-            printf("%f\t", ypr[0] * 180/M_PI);
-            printf("%f\t", ypr[1] * 180/M_PI);
-            printf("%f\t\r\n", ypr[2] * 180/M_PI);
-        #endif
-
-        #ifdef OUTPUT_READABLE_REALACCEL
-            // display real acceleration, adjusted to remove gravity
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetAccel(&aa, fifoBuffer);
-            mpu.dmpGetGravity(&gravity, &q);
-            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-            printf("areal\t");
-            printf("%f\t", aaReal.x);
-            printf("%f\t", aaReal.y);
-            printf("%f\t\r\n", aaReal.z);
-        #endif
-
-        #ifdef OUTPUT_READABLE_WORLDACCEL
-            // display initial world-frame acceleration, adjusted to remove gravity
-            // and rotated based on known orientation from quaternion
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            mpu.dmpGetAccel(&aa, fifoBuffer);
-            mpu.dmpGetGravity(&gravity, &q);
-            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
-            printf("aworld\t");
-            printf("%f\t", aaWorld.x);
-            printf("%f\t", aaWorld.y);
-            printf("%f\t\r\n", aaWorld.z);
-        #endif
-    
-        #ifdef OUTPUT_TEAPOT
-            // display quaternion values in InvenSense Teapot demo format:
-            teapotPacket[2] = fifoBuffer[0];
-            teapotPacket[3] = fifoBuffer[1];
-            teapotPacket[4] = fifoBuffer[4];
-            teapotPacket[5] = fifoBuffer[5];
-            teapotPacket[6] = fifoBuffer[8];
-            teapotPacket[7] = fifoBuffer[9];
-            teapotPacket[8] = fifoBuffer[12];
-            teapotPacket[9] = fifoBuffer[13];
-            for (int i = 0; i < 14; ++i) {
-                pc.send(teapotPacket[i]);
-            }
-            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
-        #endif
-
-        // blink LED to indicate activity
-        led1 = !led1;
+        MPU6050raw::loop();
     }
 }
\ No newline at end of file