Go Down

Topic: Trouble with getting Yaw angle from MPU6050 Gyroscope (Read 7813 times) previous topic - next topic

gilberttkd

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.

https://www.youtube.com/watch?v=4BoIE8YQwM8
https://www.youtube.com/watch?v=j-kE0AMEWy4&feature=youtu.be



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

 

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.

bms001

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.

jremington

#3
Sep 21, 2017, 05:05 pm Last Edit: Sep 21, 2017, 05:06 pm by jremington
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.


gilberttkd

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

I just read the replys from the other thread and I will try to research more about 9DOF sensor.

jremington

#5
Sep 22, 2017, 12:23 am Last Edit: Sep 22, 2017, 12:23 am by jremington
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.

gilberttkd

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 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();
    
    // 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

        

gilberttkd

Hi jremington,

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

Thanks again.

bms001

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?

gilberttkd

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?

jremington

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.

bms001

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.

gilberttkd


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:\Users\ADMINI~1\AppData\Local\Temp\ccHIgvxa.ltrans0.ltrans.o: In function `main':

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

gilberttkd

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!

noob992

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) {
    // yaw: (about Z axis)
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);

the thing is I'm not using Arduino, so I can't use the lib directly, and my main problem is I can't understand what is w and where it comes from !!

Go Up