# Arduino Forum

## Using Arduino => Programming Questions => Topic started by: gilberttkd on Sep 21, 2017, 06:42 am

Title: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 21, 2017, 06:42 am
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.

Code: [Select]
` void loop(){  read_mpu_6050_data();                                                //Read the raw acc and gyro data from the MPU-6050  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 `
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: bms001 on Sep 21, 2017, 10:41 am

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 (https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050). I found this gave pretty decent readings for Yaw.

Alternatively, get a magnetometer and use that in addition to the MPU6050.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: bms001 on Sep 21, 2017, 03:38 pm
Is there any reason why you can't use an encoder instead as suggested in your other thread (https://forum.arduino.cc/index.php?topic=500928.0)?

Seems like it might be a much simpler solution.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: jremington on Sep 21, 2017, 05:05 pm
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.

Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 22, 2017, 12:18 am
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

Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: jremington on Sep 22, 2017, 12:23 am
Quote
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.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 22, 2017, 02:52 pm
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..

Code: [Select]
`//Initialize the LCD libraryLiquidCrystal_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();          // wait for ready    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)..."));        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) return;    // 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;        #ifdef OUTPUT_READABLE_QUATERNION            // 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        #ifdef OUTPUT_READABLE_EULER            // 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        #ifdef OUTPUT_READABLE_YAWPITCHROLL            // 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        `
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 22, 2017, 03:04 pm
Hi jremington,

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

Thanks again.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: bms001 on Sep 22, 2017, 04:31 pm
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?
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 23, 2017, 12:19 am
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?
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: jremington on Sep 23, 2017, 12:34 am
Quote
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.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: bms001 on Sep 23, 2017, 12:17 pm
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).

Don't use the FIFO. Set the sensor for single readings on demand.
Or it may be simpler to just do this.
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 23, 2017, 02:33 pm

Hi bms001,

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

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h

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
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: gilberttkd on Sep 23, 2017, 02:39 pm
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!
Title: Re: Trouble with getting Yaw angle from MPU6050 Gyroscope
Post by: noob992 on Aug 17, 2020, 08:32 am
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 (https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050). 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) {