MPU-6050

[ datasheet ]

Circuit

Arduino Library

I2CDev : Download / Source
MPU6050 : Download / Source


Arduino Code

#include "MPU6050_DMP6.h"

void setup() {
  Serial.begin(9600);

  int mpuInit = mpuSetup();
  if (mpuInit == 0) {
    Serial.println("MPU6050 connected");
  } else {
    Serial.print("DMP Init Fail(");
    Serial.print(mpuInit);
    Serial.println(")");
  }
}

void loop() {
  mpuLoop();

  Serial.print("YAW: ");
  Serial.print(mpuYaw());
  Serial.print("\tPITCH: ");
  Serial.print(mpuPitch());
  Serial.print("\tROLL: ");
  Serial.println(mpuRoll());
}

MPU6050_DMP6.h

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}

MPU6050 mpu;
Quaternion quant;
VectorFloat gravity;
bool dmpReady = false;
int packetSize;
int fifoCount;
int mpuIntStatus;
byte fifoBuffer[64];
float ypr[3];

int mpuSetup() {
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

  mpu.initialize();
  if (!mpu.testConnection()) {
    return -1;
  }

  int dmpInit = mpu.dmpInitialize();
  if (dmpInit == 0) {
    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;
  } else {
    return dmpInit;
  }

  return 0;
}

void mpuLoop() {
  if (!dmpReady) return;
  while (!mpuInterrupt && fifoCount < packetSize);

  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();

  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu.resetFIFO();  //FIFO overflow
    return;
  }

  if (!(mpuIntStatus & 0x02)) {
    return;
  }

  while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  mpu.getFIFOBytes(fifoBuffer, packetSize);
  fifoCount -= packetSize;

  mpu.dmpGetQuaternion(&quant, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &quant);
  mpu.dmpGetYawPitchRoll(ypr, &quant, &gravity);

  ypr[0] = ypr[0] * 180 / M_PI;
  ypr[1] = ypr[1] * 180 / M_PI;
  ypr[2] = ypr[2] * 180 / M_PI;
}

float mpuYaw() {
  return ypr[0];
}

float mpuPitch() {
  return ypr[1];
}

float mpuRoll() {
  return ypr[2];
}