File content as of revision 1:41ec5f98abab:

/* Demo code for MPU6050 DMP
 * I thank Ian Hua.
 * Copyright (c) 2015 Match
 *
 * THE PROGRAM 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 PROGRAM OR THE USE OR OTHER DEALINGS IN
 * THE PROGRAM.
 */
 
 
// Define Necessary.
//#define OUTPUT_QUATERNION
//#define OUTPUT_EULER
#define OUTPUT_ROLL_PITCH_YAW
//#define OUTPUT_FOR_TEAPOT
//#define OUTPUT_TEMPERATURE
 
#include "MPU6050_6Axis_MotionApps20.h"
#include "mbed.h"
#include <stdio.h>

#define servotest
#ifdef servotest
PwmOut sv(P0_0);
#endif

// FIFO rate = 200Hz / (1 + this value)
// For example, 0x01 is 100Hz, 0x03 is 50Hz.
// 0x00 to 0x09
#define IMU_FIFO_RATE_DIVIDER 0x09
 
// Sample rate = 1kHz / (1 + this valye)
// For example, 4 is 200Hz.
#define IMU_SAMPLE_RATE_DIVIDER 4
 
// measuring range of gyroscope (±n deg/s)
// But other value doesn't yet support.
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
 
// measuring range of acceleration sensor (±n g)
// But other value doesn't yet support.
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
 
#define PC_BAUDRATE 921600
            

#include "USBSerial.h"
 
#define DEG_TO_RAD(x) ( x * 0.01745329 )
#define RAD_TO_DEG(x) ( x * 57.29578 )
 
 
//RawSerial pc(USBTX, USBRX);
USBSerial pc;
MPU6050 mpu;     // sda, scl pin
InterruptIn INT0(P0_9);     // INT0 pin
 
const int FIFO_BUFFER_SIZE = 128;
uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
uint16_t fifoCount;
uint16_t packetSize;
bool dmpReady;
uint8_t mpuIntStatus;
const int snprintf_buffer_size = 100;
char snprintf_buffer[snprintf_buffer_size];
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
 
struct Offset {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
}offset = {150, -350, 1000, -110, 5, 0};    // Measured values
 
struct MPU6050_DmpData {
    Quaternion q;
    VectorFloat gravity;    // g
    float roll, pitch, yaw;     // rad
}dmpData;
 
long map(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

bool Init();
void dmpDataUpdate();
 
 
int main() {
    MBED_ASSERT(Init() == true);
    
    while(1) {
        
    }
}
 
 
bool Init() {
    //pc.baud(PC_BAUDRATE);
    
    while(!pc.readable());
            pc.getc();
    
    INT0.mode(PullDown);
    INT0.fall(dmpDataUpdate);
    
    mpu.initialize();
    if (mpu.testConnection()) {
        pc.puts("MPU6050 test connection passed.\n");
    } else {
        pc.puts("MPU6050 test connection failed.\n");
        return false;
    }
    if (mpu.dmpInitialize() == 0) {
        pc.puts("succeed in MPU6050 DMP Initializing.\n");
    } else {
        pc.puts("failed in MPU6050 DMP Initializing.\n");
        return false;
    }
    mpu.setXAccelOffset(offset.ax);
    mpu.setYAccelOffset(offset.ay);
    mpu.setZAccelOffset(offset.az);
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setXGyroOffsetUser(offset.gx);
    mpu.setYGyroOffsetUser(offset.gy);
    mpu.setZGyroOffsetUser(offset.gz);
    mpu.setDMPEnabled(true);    // Enable DMP
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;    // Enable interrupt.
    
    pc.puts("Init finish!\n");
 
 #ifdef servotest
    sv.pulsewidth_us(1450);
 #endif
 
    return true;
}
 
 
void dmpDataUpdate() {
    // Check that this interrupt has enabled.
    if (dmpReady == false) return;
    
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    
    // Check that this interrupt is a FIFO buffer overflow interrupt.
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
        //pc.puts("FIFO overflow!\n");
        return;
        
    // Check that this interrupt is a Data Ready interrupt.
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
    #ifdef OUTPUT_QUATERNION
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Quaternion : w=%f, x=%f, y=%f, z=%f\n", dmpData.q.w, dmpData.q.x, dmpData.q.y, dmpData.q.z ) < 0 ) return;
        pc.puts(snprintf_buffer);
    #endif
        
    #ifdef OUTPUT_EULER
        float euler[3];
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        mpu.dmpGetEuler(euler, &dmpData.q);
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Euler : psi=%fdeg, theta=%fdeg, phi=%fdeg\n", RAD_TO_DEG(euler[0]), RAD_TO_DEG(euler[1]), RAD_TO_DEG(euler[2]) ) < 0 ) return;
        pc.puts(snprintf_buffer);
    #endif
        
    #ifdef OUTPUT_ROLL_PITCH_YAW
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
        float rollPitchYaw[3];
        mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
        dmpData.roll = rollPitchYaw[2];
        dmpData.pitch = rollPitchYaw[1];
        dmpData.yaw = rollPitchYaw[0];
        
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return;
        pc.puts(snprintf_buffer);
                 
#ifdef servotest
        int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
        if(servoPulse > 1450) servoPulse = 1450;
        if(servoPulse < 500) servoPulse = 500;
        sv.pulsewidth_us(servoPulse);
#endif
    #endif
        
    #ifdef OUTPUT_FOR_TEAPOT
        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 (uint8_t i = 0; i < 14; i++) {
            pc.putc(teapotPacket[i]);
        }
    #endif
        
    #ifdef OUTPUT_TEMPERATURE
        float temp = mpu.getTemperature() / 340.0 + 36.53;
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
        pc.puts(snprintf_buffer);
    #endif
        
        pc.puts("\n");
    }
}