File content as of revision 6:6300d9561dfd:

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

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

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}

#ifndef M_PI
#define M_PI 3.1415
#endif

I2C i2c();

int main()
{
   pc.baud(115200);
    
    mpu.initialize();

    // verify connection
    pc.printf("Testing device connections...\r\n");
    
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult)
    {
        pc.printf("MPU6050 test passed \r\n");
    }
    else
    {
        pc.printf("MPU6050 test failed \r\n");
    }  
    
    // load and configure the DMP
    pc.printf("Initializing DMP...\n");
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(0);
    mpu.setYGyroOffset(0);
    mpu.setZGyroOffset(0);
    mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
    
    if (devStatus==0)
    {
        pc.printf("OK!\r\n");
        mpu.setDMPEnabled(true);
        
        mpuIntStatus = mpu.getIntStatus();
        
        packetSize = mpu.dmpGetFIFOPacketSize();
    }
    else
    {
        pc.printf("Failed with code %d\r\n", devStatus);
        while(1);
    }
    
    // Main prog loop
    while (1)
    {
        wait_us(500);
        
        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();
            //pc.printf("FIFO overflow!");
        }
        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;   
            
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            /*pc.printf("ypr\t");
            pc.printf("%f3.2",ypr[0] * 180/M_PI);
            pc.printf("\t");
            pc.printf("%f3.2",ypr[1] * 180/M_PI);
            pc.printf("\t");
            pc.printf("%f3.2\n",ypr[2] * 180/M_PI);*/
            
        }
    }
}