YAW Calculation from MPU6050

Using Arduino Sensors
1

I am trying to calculate yaw pitch and roll for my Quadcopter project. I have taken the code from Here. I have been succeeded in calculating roll and pitch in degrees and applying complimentary filter to these values and these values are very near to the values calculated by Jeff Rowberg's code. I want to calculate the yaw value from this RAW data instead of using Jeff's code. I know yaw calculated by RAW data will not be that accurate as Jeff's but can anyone please tell me the formula or maths behind calculating the yaw value from accelerometer and gyro RAW data? here is the code I am using

#include<Wire.h>
const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double compAngleX,compAngleY,/*gzangle,*/compAnglez,timer;
  double accXangle ,accYangle,acczangle ,gyroXrate ,gyroYrate,gyroZrate;
  double gyroXAngle, gyroYAngle, gyroZAngle;
  // float rgyro,w;
  int ap=0.955;

  void setup()
{
   Serial.begin(115200);
/////////////////////// SENSOR READING//////////
 Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  ///////////////////////////////////////
}
void loop()
{

  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,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)



 accXangle = (atan2(AcY, AcZ) * RAD_TO_DEG);
 accYangle = (atan2(AcX, AcZ) * RAD_TO_DEG);
acczangle = (atan2(AcX,AcY) * RAD_TO_DEG);/* my attempt to calculate yaw but not correct*/
 gyroXrate = GyX / 16.5;
 gyroYrate = GyY / 16.5;
 gyroZrate = GyZ/ 16.5;
 timer = millis();
 //angular position
 gyroXAngle += gyroXrate * (millis()-timer)/1000;
 gyroYAngle += gyroYrate * (millis()-timer)/1000;
 gyroZAngle += gyroZrate * (millis()-timer)/1000;/* my attempt to calculate yaw but not correct*/
//---------------------------\\
 //COMPLIMENTRY FILTER STARTED\\
 //---------------------------\\

 compAngleX = ap * (compAngleX + gyroXAngle) + (1-ap) * accXangle;
 compAngleY = ap * (compAngleY + gyroYAngle) + (1-ap) * accYangle;
 compAnglez = ap * (compAnglez + gyroZAngle) + (1-ap) * acczangle; /*yaw but not correct*/
 //---------------------------\\
 //COMPLIMENTRY FILTER ENDED  \\
 //---------------------------\\

   Serial.print("ypr\t");
            Serial.print(compAnglez);
            Serial.print("\t");
            Serial.print(compAngleY);
            Serial.print("\t");
            Serial.println(compAngleX);
}

Please anyone ?

2

Hi,

There is very easy way to compute the Yaw angle:

First, you must know the original position, for example, at 0º, and then, integrate the Yaw gyro rate along the time, this is to say:

Yaw = Yaw + Yaw_rate*dt

Where dt is the time difference between two steps.

This method have some drawbacks:

1 - Error become accumulative along the time, so accuracy may be consider zero.
2 - You need to known a high accuracy initial angle.

You can not filter it with a Kalman or a complementary filter, because the solution of the transformation matrix only provides roll and pitch, so you must use a Magnetometer (there are cheap models).

For further information, I attach to this message a document (Angulo_Acelerometro, which means Accelerometer_Angle in English) where there are all explained, I hope it will be useful to you.

Regards.

Angulo_Acelerometro.pdf (581 KB)

3

Thankyou for your post. it was helping. I want to ask is there any method to calculate yaw angle without using Magnetometer ? I mean many of people are using mpu6050 for roll oitch and yaw calculations. How does they do it?

4

i could not do it also how can i get the z angle :frowning:

5

Gravity, measured by the accelerometer, defines down for pitch and roll angles. To calculate yaw, you need some other reference, for example, a magnetometer to determine North.

What is a "z angle"?

6

jremington:
Gravity, measured by the accelerometer, defines down for pitch and roll angles. To calculate yaw, you need some other reference, for example, a magnetometer to determine North.

What is a "z angle"?

Actually it is not.
The codes with libs gets yaw angle.
I tried almost everything with my code which is too likely with this but either I get zero or get constant value.