# Trouble with getting Yaw angle from MPU6050 Gyroscope

Hi, I am trying to build a satellite dish with positioning control using my mobile phone for my project. I am using Arduino-uno, Bluetooth receivers and servo motors/gearbox from Servo-city. I have done my research and was able make the interface between my mobile phone and Servomotors work, the satellite dish is moving left/right (azimuth/Yaw) and up/down(elevation/Pitch) with the use of my mobile phone.

I am now on the second stage of doing the Azimuth and Elevation monitoring. To do this, I am trying to use Arduino-uno and MPU6050 gyroscope. I have found a code online, the code works perfect but it is monitoring Roll and Pitch, but what I need is Yaw and Pitch.

I am trying to edit the code to measure the Yaw and Pitch . I tried to replace the x-axis with the z-axis but the result is not accurate. I believed there is a mathematical formula for this but I don’t know.

Does anybody have done this before? Kindly help me please. Thank you.

I have attached the Pitch, Roll and Yaw reference for common understanding.

Here’s the youtube video for this and code as well.

``````void loop(){

gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value

//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_z * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable

//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_x * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_x * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel

//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle
angle_roll_acc = asin((float)acc_z/acc_total_vector)* -57.296;       //Calculate the roll angle

//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= +003.8;                                              //Accelerometer calibration value for pitch
angle_roll_acc -= +002.7;                                               //Accelerometer calibration value for roll

if(set_gyro_angles){                                                 //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{                                                                //At first start
angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true;                                            //Set the IMU started flag
}

//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value
``````

With that approach, it is unlikely that you will get accurate values for Yaw while the Yaw axis is vertical.

Look at the MPU6050_DMP6 example here. I found this gave pretty decent readings for Yaw.

Alternatively, get a magnetometer and use that in addition to the MPU6050.

Is there any reason why you can't use an encoder instead as suggested in your other thread?

Seems like it might be a much simpler solution.

Gyros can't be used to measure absolute angles, instead they measure the rate of change of angle.

To determine absolute orientation, you can use an accelerometer and magnetometer, but the magnetometer will be confused by nearby iron objects or magnetic fields and must be calibrated in place.

Hi bms001, I initially use HMC5883L magnetometer but this is a compass and it will not give me elevation angle. If I combine it with the MPU6050 I will be needing 2 arduino to control each of them and 2 LCD screen to out put the angles.

HMC5883L + arduino + LCD screen to output Azimuth
MPU6050 + arduino + LCD screen to output elevation

If I combine it with the MPU6050 I will be needing 2 arduino to control each of them

No, one Arduino can read both sensors and combine the information.

Do not double post.

Hi bms001,

I tried this MPU6050_DMP6 example that you gave me and its quite accurate. I think this will work for my project.

I just need to edit the program to print it in the 20x4 LCD that I have.

Would you mind to check if I am doing the right thing in editing because I am having “FIFO overflow” error.

Thank you…

``````//Initialize the LCD library
LiquidCrystal_I2C lcd(0x3F,2,1,0,4,5,6,7);

void setup() {
[b] [color=red] lcd.begin(20,4);
lcd.setBacklightPin(3,POSITIVE);
lcd.setBacklight(HIGH);

// 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 Ardunio
// 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
lcd.print(F("Initializing I2C..."));
lcd.setCursor(0,0);
delay(1500);
lcd.clear();

mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);

// verify connection
lcd.print(F("Testing device..."));
lcd.setCursor(0,0);
delay(1500);
lcd.clear();

lcd.print(mpu.testConnection() ? F("MPU6050 connection  successful") : F("MPU6050 connection failed"));
lcd.setCursor(0,0);
delay(1500);
lcd.clear();

Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available());                 // wait for data
while (Serial.available() && Serial.read()); // empty buffer again

// 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) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
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..."));

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

// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif

// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif

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

Hi jremington,

Thank you for that info and my apology for the double post. I won’t do it again.

Thanks again.

gilberttkd:
Would you mind to check if I am doing the right thing in editing because I am having "FIFO overflow" error.

In theory it's because you're reading the FIFO slower that data is being put in it. Can you try printing to the Serial monitor instead of the LCD and see if you still get FIFO overflow?

Hi bms001,

I don't have an error when printing using Serial Monitor, everything is good.
But I need to print it in the 20x4 LCD so that people can see.

Yes, you are right, the input is so fast that is causing the FIFO overflow.

Do you know how can I overcome this? I mean to make the input slower?

Do you know how can I overcome this? I mean to make the input slower?

Don’t use the FIFO. Set the sensor for single readings on demand.

gilberttkd:
Do you know how can I overcome this? I mean to make the input slower?

To change the FIFO refresh rate you can modify line 305 in the 'MPU6050_6Axis_MotionApps20.h' file. There are comments which explain how to do so.

You could also try writing to the LCD less frequently (though perhaps a single write is enough to cause the overflow, I don't know).

jremington:
Don't use the FIFO. Set the sensor for single readings on demand.

Or it may be simpler to just do this.

Hi bms001,

I am using this code below but am having this error, do you know how to solve this?

Error message:

C:\Program Files (x86)\Arduino\hardware\arduino\avr\cores\arduino/main.cpp:43: undefined reference to `setup'

C:\Program Files (x86)\Arduino\hardware\arduino\avr\cores\arduino/main.cpp:46: undefined reference to `loop'

collect2.exe: error: ld returned 1 exit status

Hi jremington,

Thanks for your advise, Would you mind to show me how to Set the sensor for single readings on demand?

I am quite new so it would be great help for me to learn. Thanks!

bms001:
With that approach, it is unlikely that you will get accurate values for Yaw while the Yaw axis is vertical.

Look at the MPU6050_DMP6 example here. I found this gave pretty decent readings for Yaw.

Alternatively, get a magnetometer and use that in addition to the MPU6050.

Hello sir,
there is a function in the library,
dmpGetYawPitchRoll(float data, Quaternion q, VectorFloat gravity) {