Converting Raw data from MPU 6050 to YAW,PITCH AND ROLL

I have found this piece of code:-

#include <Wire.h>

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

void setup() {
Serial.begin(9600);
Wire.begin();
setupMPU();
}

void loop() {
recordAccelRegisters();
recordGyroRegisters();
printData();
delay(100);
}

void setupMPU(){
Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
Wire.endTransmission();
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
Wire.write(0b00000000); //Setting the accel to +/- 2g
Wire.endTransmission();
}

void recordAccelRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x3B); //Starting register for Accel Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processAccelData();
}

void processAccelData(){
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
}

void recordGyroRegisters() {
Wire.beginTransmission(0b1101000); //I2C address of the MPU
Wire.write(0x43); //Starting register for Gyro Readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
processGyroData();
}

void processGyroData() {
rotX = gyroX / 131.0;
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
}

void printData() {
Serial.print(“Gyro (deg)”);
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.print(rotY);
Serial.print(" Z=");
Serial.print(rotZ);
Serial.print(" Accel (g)");
Serial.print(" X=");
Serial.print(gForceX);
Serial.print(" Y=");
Serial.print(gForceY);
Serial.print(" Z=");
Serial.println(gForceZ);
}

How do I calculate YAW,PITCH and ROLL from this ?

Read this tutorial on how to use a complimentary filter to find pitch and roll Euler Angles.

Yaw is much more difficult and usually requires a magnetometer (which an MPU6050 doesn't have). You can approximate relative yaw based off filtered z-axis gyro data, but you won't know which azimuth you are facing in (in case you are also using GPS data).

Please use code tags when posting code.

For a general overview on how to determine yaw, pitch and roll angles for a model, see http://www.chrobotics.com/library

i have found this program to obtain raw values from mpu6050. How to apply complemetary filter to these values and obtain yaw,pitch and roll values for quadcopter ?

#include <Wire.h>

long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;

long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();
}


void loop() {
  recordAccelRegisters();
  recordGyroRegisters();
  printData();
  delay(100);
}

void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission(); 
}

void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}

void processAccelData(){
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0; 
  gForceZ = accelZ / 16384.0;
}

void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}

void processGyroData() {
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0; 
  rotZ = gyroZ / 131.0;
}

void printData() {
  Serial.print("Gyro (deg)");
  Serial.print(" X=");
  Serial.print(rotX);
  Serial.print(" Y=");
  Serial.print(rotY);
  Serial.print(" Z=");
  Serial.print(rotZ);
  Serial.print(" Accel (g)");
  Serial.print(" X=");
  Serial.print(gForceX);
  Serial.print(" Y=");
  Serial.print(gForceY);
  Serial.print(" Z=");
  Serial.println(gForceZ);
}

@JitenSinha, do not cross-post. Threads merged.

Did you not read the link I posted about how to implement a complimentary filter?!?

I have written this code :

#include <Wire.h>

#define A 0.962
#define dt 0.020

int calx,caly;

double accel_x_cal,accel_y_cal,accel_z_cal;
double accelX, accelY, accelZ;

float rollangle,pitchangle;
float roll,pitch,yaw;

double gyroX, gyroY, gyroZ;
double gyro_x_cal,gyro_y_cal,gyro_z_cal;


void setup() {
  Serial.begin(115200);
  Wire.begin();

  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission(); 
 
  Serial.println("Calibrating Accelerometer......");
  for(calx=1;calx<=2000;calx++)
  {
     recordAccelRegisters();
     accel_x_cal += accelX;                      
     accel_y_cal += accelY;      
     accel_z_cal += accelZ;
  }
  Serial.println("Calibrating Gyroscope......");
  for(caly=1;caly<=2000;caly++)
  {
    recordGyroRegisters();
    gyro_x_cal += gyroX;                      
    gyro_y_cal += gyroY;      
    gyro_z_cal += gyroZ;
  }
  Serial.println("Calibration Done..!!!");
  gyro_x_cal /= 2000;                             
  gyro_y_cal /= 2000;                            
  gyro_z_cal /= 2000;                              

   gyro_x_cal /= 2000;                             
  gyro_y_cal /= 2000;                            
  gyro_z_cal /= 2000;                              
}

void loop() {
 recordAccelRegisters();
 recordGyroRegisters();

  accelX = accelX / 16384.0;
  accelY = accelY / 16384.0; 
  accelZ = accelZ / 16384.0;

  gyroX = gyroX / 131.0;
  gyroY = gyroY / 131.0; 
  gyroZ = gyroZ / 131.0;

  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET
  
  //Using Complemetary Filter
  roll=A*(roll+gyroX*dt)+(1-A)*rollangle;
  pitch=A*(pitch+gyroY*dt)+(1-A)*pitchangle;
  
  yaw=gyroZ;

  Serial.print(" ROLL=");
  Serial.print(roll);
  Serial.print(" angle");
  Serial.print("    PITCH=");
  Serial.print(pitch);
  Serial.print(" angle");
  Serial.print("    YAW=");
  Serial.print(yaw);
  Serial.println(" deg/s");
  
}


void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelX -= accel_x_cal; 
  accelY = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelY -= accel_y_cal; 
  accelZ = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelZ -= accel_z_cal; 
  
}


void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read();
  if(caly == 2000)gyroX -= gyro_x_cal; 
  gyroY = Wire.read()<<8|Wire.read(); 
  if(caly == 2000)gyroY -= gyro_y_cal; 
  gyroZ = Wire.read()<<8|Wire.read();
  if(caly == 2000)gyroZ -= gyro_z_cal; 
  
}

Its working fine but the problem is its giving very slow data when baud rate is set to 9600,the angle slowly gradually changes when tilting the sensor but when the rate is set to 115200,the data speed is normal.What should be done ?

Serial printing is slow, especially at 9600 Baud.

It is a serious error to do this twice:

  gyro_x_cal /= 2000;                             
  gyro_y_cal /= 2000;                           
  gyro_z_cal /= 2000;

The accelerometer calibration is completely wrong. See this tutorial on how to calibrate an accelerometer.

@jremington is it okay if i remove the calibration for acclerometer entirely and only apply the complementary filter to gain the angles.Is the rest of the program alright ?Is the gyro calibration loop okay ?

#include <Wire.h>

#define A 0.962
#define dt 0.020

int calx,caly;

double accel_x_cal,accel_y_cal,accel_z_cal;
double accelX, accelY, accelZ;

float rollangle,pitchangle;
float roll,pitch,yaw;

double gyroX, gyroY, gyroZ;
double gyro_x_cal,gyro_y_cal,gyro_z_cal;


void setup() {
  Serial.begin(115200);
  Wire.begin();

  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission(); 
 
  Serial.println("Calibrating Accelerometer......");
  for(calx=1;calx<=2000;calx++)
  {
     recordAccelRegisters();
     accel_x_cal += accelX;                      
     accel_y_cal += accelY;      
     accel_z_cal += accelZ;
  }
  Serial.println("Calibrating Gyroscope......");
  for(caly=1;caly<=2000;caly++)
  {
    recordGyroRegisters();
    gyro_x_cal += gyroX;                      
    gyro_y_cal += gyroY;      
    gyro_z_cal += gyroZ;
  }
  Serial.println("Calibration Done..!!!");
  accel_x_cal /= 2000;                             
  accel_y_cal /= 2000;                            
  accel_z_cal /= 2000;                              

   gyro_x_cal /= 2000;                             
  gyro_y_cal /= 2000;                            
  gyro_z_cal /= 2000;                              
}

void loop() {
 recordAccelRegisters();
 recordGyroRegisters();

  accelX = accelX / 16384.0;
  accelY = accelY / 16384.0; 
  accelZ = accelZ / 16384.0;

  gyroX = gyroX / 131.0;
  gyroY = gyroY / 131.0; 
  gyroZ = gyroZ / 131.0;

  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET
  
  //Using Complemetary Filter
  roll=A*(roll+gyroX*dt)+(1-A)*rollangle;
  pitch=A*(pitch+gyroY*dt)+(1-A)*pitchangle;
  
  yaw=gyroZ;

  Serial.print(" ROLL=");
  Serial.print(roll);
  Serial.print(" angle");
  Serial.print("    PITCH=");
  Serial.print(pitch);
  Serial.print(" angle");
  Serial.print("    YAW=");
  Serial.print(yaw);
  Serial.println(" deg/s");
  
}


void recordAccelRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Accel Registers (3B - 40)
  while(Wire.available() < 6);
  accelX = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelX -= accel_x_cal; 
  accelY = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelY -= accel_y_cal; 
  accelZ = Wire.read()<<8|Wire.read(); 
  if(calx == 2000)accelZ -= accel_z_cal; 
  
}


void recordGyroRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,6); //Request Gyro Registers (43 - 48)
  while(Wire.available() < 6);
  gyroX = Wire.read()<<8|Wire.read();
  if(caly == 2000)gyroX -= gyro_x_cal; 
  gyroY = Wire.read()<<8|Wire.read(); 
  if(caly == 2000)gyroY -= gyro_y_cal; 
  gyroZ = Wire.read()<<8|Wire.read();
  if(caly == 2000)gyroZ -= gyro_z_cal; 
  
}

Is it okay now ?

The accelerometer calibration is still completely wrong.

#include <Wire.h>

int cal_int;

double temperature;

double accelX, accelY, accelZ;

float roll,pitch,yaw;

double gyroX, gyroY, gyroZ;
double gyro_x_cal,gyro_y_cal,gyro_z_cal;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  setupMPU();
  Serial.println("Calibrating Gyroscope......");
  for(cal_int=1;cal_int<=2000;cal_int++)
  {
    recordRegisters();      
    gyro_z_cal += gyroZ;
  }
  Serial.println("Calibration Done..!!!");
   gyro_z_cal /= 2000;  
  }


void loop() {
  recordRegisters();
  accelX = accelX / 4096.0;
  accelY = accelY / 4096.0; 
  accelZ= accelZ / 4096.0;

  gyroZ = gyroZ / 65.5;

  roll=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitch=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET
  yaw=gyroZ;

  Serial.print("ROLL  ");
  Serial.print(roll);
  Serial.print(" degrees");
  Serial.print("  PITCH  ");
  Serial.print(pitch);
  Serial.print(" degrees");
  Serial.print("  YAW ");
  Serial.print(yaw);
  Serial.println(" dps");
 
}

void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x08); //Setting the gyro to full scale +/- 500deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0x10); //Setting the accel to +/- 8g
  Wire.endTransmission(); 
  
    Wire.beginTransmission(0b1101000);                                      //Start communication with the address found during search
    Wire.write(0x1A);                                                          //We want to write to the CONFIG register (1A hex)
    Wire.write(0x03);                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
    Wire.endTransmission();                                                    //End the transmission with the gyro    
}

void recordRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,14); //Request Accel Registers (3B - 40)
  while(Wire.available() < 14);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  temperature=Wire.read()<<8|Wire.read();
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ

  if(cal_int == 2000)
  {
    gyroZ -= gyro_z_cal;                                       //Only compensate after the calibration.
  }
}

Will this program be sufficient to obtain yaw,pitch and roll angles for the quadcopter ?

#include<Wire.h>

double accelX,accelY,accelZ,temperature,gyroX,gyroY,gyroZ,gyro_x_cal,gyro_y_cal,gyro_z_cal; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int.  We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double roll, pitch ,yaw; //These are the angles in the complementary filter
float rollangle,pitchangle;
int cal_int;
void setup() {
  // Set up MPU 6050:
  Serial.begin(115200);
  Wire.begin();

  setupMPU();   
  Serial.println("Calibrating Gyroscope......");
  for(cal_int=1;cal_int<=2000;cal_int++)
  { 
   recordRegisters();
   gyro_x_cal += gyroX;
   gyro_y_cal  += gyroY ;
   gyro_z_cal += gyroZ;
  }
  Serial.println("Calibration Done..!!!");
  gyro_x_cal /= 2000;
  gyro_y_cal /= 2000;
  gyro_z_cal /= 2000;
  
  //start a timer
  timer = micros();
}

void loop() {
//Now begins the main loop. 
  //Collect raw data from the sensor.
  recordRegisters();
  gyroX = gyroX / 65.5;
  gyroY = gyroY / 65.5;
  gyroZ = gyroZ / 65.5;

  accelX = accelX / 4096.0;
  accelY = accelY / 4096.0; 
  accelZ= accelZ / 4096.0;
  
  double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
  timer = micros(); //start the timer again so that we can calculate the next dt.

  //the next two lines calculate the orientation of the accelerometer relative to the earth and convert the output of atan2 from radians to degrees
  //We will use this data to correct any cumulative errors in the orientation that the gyroscope develops.
  rollangle=atan2(accelY,accelZ)*180/PI; // FORMULA FOUND ON INTERNET
  pitchangle=atan2(accelX,sqrt(accelY*accelY+accelZ*accelZ))*180/PI; //FORMULA FOUND ON INTERNET

  
  

  //THE COMPLEMENTARY FILTER
  //This filter calculates the angle based MOSTLY on integrating the angular velocity to an angular displacement.
  //dt, recall, is the time between gathering data from the MPU6050.  We'll pretend that the 
  //angular velocity has remained constant over the time dt, and multiply angular velocity by 
  //time to get displacement.
  //The filter then adds a small correcting factor from the accelerometer ("roll" or "pitch"), so the gyroscope knows which way is down. 
  roll = 0.99 * (roll+ gyroX * dt) + 0.01 * rollangle; // Calculate the angle using a Complimentary filter
  pitch = 0.99 * (pitch + gyroY * dt) + 0.01 * pitchangle; 
  yaw=gyroZ;
  
  
  Serial.print("roll  ");
  Serial.print(roll);
  Serial.print("   pitch  ");
  Serial.print(pitch);
  Serial.print("   yaw    ");
  Serial.println(yaw);

}

void setupMPU(){
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();  
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4) 
  Wire.write(0x08); //Setting the gyro to full scale +/- 500deg./s 
  Wire.endTransmission(); 
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5) 
  Wire.write(0x10); //Setting the accel to +/- 8g
  Wire.endTransmission(); 
  
  Wire.beginTransmission(0b1101000);                                      //Start communication with the address found during search
  Wire.write(0x1A);                                                          //We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  Wire.endTransmission();                                                    //End the transmission with the gyro    
}
void recordRegisters() {
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,14); //Request Accel Registers (3B - 40)
  while(Wire.available() < 14);
  accelX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ
  temperature=Wire.read()<<8|Wire.read();
  gyroX = Wire.read()<<8|Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read()<<8|Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read()<<8|Wire.read(); //Store last two bytes into accelZ

  if(cal_int == 2000)
  {
    gyroX -= gyro_x_cal;
    gyroY -= gyro_y_cal;
    gyroZ -= gyro_z_cal;
   
  }
}

Here is the final program that i wrote . Please verify if its okay or not .

Does the program report the correct pitch and roll angles?

the angles are somewhat okay but the problem with the speed persists . When a change is made in the angle, the change in the serial monitor takes place slowly and gradually.

Change the weights in the complementary filter.

you can find this code too from arduino examples of mpu6050 named mpu6050_DM6 for finding yaw pitch roll

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include “I2Cdev.h”

#include “MPU6050_6Axis_MotionApps20.h”
//#include “MPU6050.h” // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include “Wire.h”
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // ← use for AD0 high

#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// 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

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘\r’, ‘\n’ };

// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================

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

// ================================================================
// === INITIAL SETUP ===
// ================================================================

void setup() {
// join I2C bus (I2Cdev library doesn’t do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it’s
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.

// initialize device
Serial.println(F(“Initializing I2C devices…”));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);

// verify connection
Serial.println(F(“Testing device connections…”));
Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));

// load and configure the DMP
Serial.println(F(“Initializing DMP…”));
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it’s ready
Serial.println(F(“Enabling DMP…”));
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
Serial.print(F(“Enabling interrupt detection (Arduino external interrupt “));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(”)…”));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it’s okay to use it
Serial.println(F(“DMP ready! Waiting for first interrupt…”));
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)
Serial.print(F(“DMP Initialization failed (code “));
Serial.print(devStatus);
Serial.println(F(”)”));
}

// configure LED for output
pinMode(LED_PIN, OUTPUT);
}

// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================

void loop() {
// if programming failed, don’t try to do anything
if (!dmpReady)
mpu.reset();
return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet

// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print(“ypr\t”);
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);

}
}

Hi, I have been working on the same thing. However, I found a short example from the Arduino website that will print the raw data.
And have been trying to convert the rate of rotation to degree position, by simply multiplying time changed with the rate of rotation.

#include<Wire.h>
#define SDA 22//for esp32, commented out if you're not using this board

#define SCL 21//for esp32, commented out if you're not using this board


const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float GyrX=0;
float GyrY=0;
float GyrZ=0;
float dt;
unsigned long MillisOld;

void setup(){
Wire.begin(SDA, SCL);//for esp32, commented out if you're not using this board
//Wire.begin();//for other board
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
MillisOld = millis();
Serial.begin(115200);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=GyX/131.;
GyY=GyY/131.;
GyZ=GyZ/131.;

dt = (millis()-MillisOld)/1000.;
MillisOld = millis();
GyrX = (GyrX + GyX*dt);
GyrY = (GyrY + GyY*dt);
GyrZ = (GyrZ + GyZ*dt);
/*
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
*/
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet

Serial.print(" | GyX = "); Serial.print(GyrX);
Serial.print(" | GyY = "); Serial.print(GyrY);
Serial.print(" | GyZ = "); Serial.println(GyrZ);

}

Your math was somewhat incorrect too. to find Pitch angle should be the angle between X and Z axis which will be

atan2(accelZ,accelX)*180/PI;

for Roll angle should be the angle between Y and Z axis which will be

atan2(accelZ,accelY)*180/PI;

And to find yaw it should be the angle between X and VectorY+Z which will be

atan2(sqrt(accelY*accelY+accelZ*accelZ),accelX)*180/PI;

also, you shouldn't divide 2000 then 131.