Ah yes I did add an alternate function for get gravity elsewhere. I made corrections to use the native version found in the MPU6050_6Axis_MotionApps20.h
now with that working 
your values will be drifting a little
Get the calibration values for the MPU6050 calibration code I'm attaching also
you will find the MPU6050 much more stable now.
How this code works.
your MPU6050 triggers an interrupt about ever 10ms to let you know the DMP is ready. it triggers the i2c bus request to get the 42 bytes of data stored in the FIFO buffer which has lots of data available for you to use.
Default MotionApps v2.0 42-byte FIFO packet structure:
[QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ]
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
The functions like the getGravity use the QUAT W,X,Y,Z to calculate the values. the resulting output with my code is angles in Degrees but I convert this from radians so both are easily available for you to use.
Look for the MPUMath() funtion
// ================================================================
// === MPU Math ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Yaw = (ypr[0] * 180.0 / M_PI);
Pitch = (ypr[1] * 180.0 / M_PI);
Roll = (ypr[2] * 180.0 / M_PI);
getYawPitchRollInDeg(ypr, &q, &gravity);
DPRINTSTIMER(100) {
DPRINTSFN(15, " W:", q.w, -6, 4);
DPRINTSFN(15, " X:", q.x, -6, 4);
DPRINTSFN(15, " Y:", q.y, -6, 4);
DPRINTSFN(15, " Z:", q.z, -6, 4);
DPRINTSFN(15, " Yaw:", Yaw, -6, 2);
DPRINTSFN(15, " Pitch:", Pitch, -6, 2);
DPRINTSFN(15, " Roll:", Roll, -6, 2);
DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);
DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);
DPRINTSFN(15, " Roll:", ypr[2], -6, 2);
DPRINTLN();
}
}
This routine will only trigger every 10ms when real data is received.
It conveniently provides you with angles in degrees ready for you to use.
Note The DPRINTS*** arn macros to provide easy Debugging output to the serial monitor
they are just glorified Serial.println() and can be deleted and replaced with your code.
Example Macro:
#define DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" "); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision
Serial prints a tab Prints the name and a space then prints the number by converting it to a string using dtostrf()
lots of stuff done with this one macro
Also note that the code I provided is not blocking so you have lots of processing time available to do other stuff
void loop() {
if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
GetDMP(); // only runs every 10ms when the interrupt is triggered
}
// place other non blocking code here
}
Z
MPU6050_Latest_code_Leonardo_int_pin_7.ino (7.88 KB)
MPU6050_calibration.ino (7.64 KB)