File content as of revision 6:b272bd888e98:

#include "mbed.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "shared.h"

#ifndef M_PI
#define M_PI 3.1415
#endif

MPU6050 mpu;

// 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
int count = 0;

int main()
{
    pc.baud(115200);
    pc.printf("MPU6050 initialize \r\n");

    mpu.initialize();
    pc.printf("MPU6050 testConnection \r\n");

    if(mpu.testConnection())
    {
        pc.printf("MPU6050 test passed \r\n");
    }
    else
    {
        pc.printf("MPU6050 test failed \r\n");
    }
    
    devStatus = mpu.dmpInitialize();
 
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(-61);
    mpu.setYGyroOffset(-127);
    mpu.setZGyroOffset(19);
    mpu.setZAccelOffset(16282); // 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
        pc.printf("Enabling DMP...\r\n");
        mpu.setDMPEnabled(true);
 
        mpuIntStatus = mpu.getIntStatus();
 
        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        pc.printf("DMP ready!\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("DMP Initialization failed (code %d )\r\n", devStatus);
        while (1);
    }
    
    while (1);
    
    while (1)
    {
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        
        // Check overflow
        if ((mpuIntStatus & 0x10) || fifoCount == 1024)
        {
            // reset so we can continue cleanly
            mpu.resetFIFO();
            pc.printf("FIFO overflow!\r\n");
        }
        
        if (mpuIntStatus & 0x02)
        {
            // wait for correct available data length, should be a VERY short wait
            while (fifoCount < packetSize)
            {
                fifoCount = mpu.getFIFOCount();
            }
            
            mpu.getFIFOBytes(fifoBuffer, packetSize);
            
            fifoCount -= packetSize;
        }
            
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetEuler(euler, &q);
        
        for (int i = 0; i < 3; i++)
        {
            euler[i] *= 180/M_PI;
        }
        
        if (count++ > 20)
        {
            pc.printf("euler\t%d\t%d\t%d\r\n", euler[0], euler[1], euler[2]);
            count = 0;
        }
    }

}