Converting rotation angles from MPU6050 to roll/pitch/yaw

Projects Programming
1

Hello. So I wrote a simple code, which uses raw gyro values from MPU6050, calculates the offset and integrates them. It is far from ideal, but the drift is surprisingly small. However, the problem is that I get rotation angles around all three axes, which move together with the MPU6050 itself. I want to get roll, pitch and yaw values from that, but I can't seem to figure out how. I have servos attached for visualization.

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include  <Servo.h>

Servo X;
Servo Y;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;
float x=0;
int cx=0;
float bx=0;
float rx=0;
float y=0;
int cy=0;
float by=0;
float ry=0;
float z=0;
int cz=0;
float bz=0;
float rz=0;
void setup()
{
  Wire.begin();
  Serial.begin(9600);

  X.attach(11);
  Y.attach(12);

  Serial.println("Initialize MPU");
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
}

void loop()
{
if(millis()<5000){
  delay(5000);
  for(int i=0; i<10000; i++){
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    bx=bx+gx;
    by=by+gy;
    bz=bz+gz;
  }
  bx=bx/10000;
  by=by/10000;
  bz=bz/10000;
}

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  x=x+gx-bx;
  y=y+gy-by;
  z=z+gz-bz;
  cx=map(x, -750000, 750000, -90, 90);
  cy=map(y, -750000, 750000, -90, 90);
  cz=map(z, -750000, 750000, -90, 90);
  Serial.print(cx);
  Serial.print(" , ");
  Serial.print(cy);
  Serial.print(" , ");
  Serial.println(cz);
  X.write(cx+90);
  Y.write(cy+90);
}
2

After some though I realized I need to convert angular velocities directly into roll pitch and yaw, and depending one the orientation, integrate them differently. Still no idea how. Also I realized that integration (x=x+gx-bx thing) needs to be done at precise fixed intervals, so it is consistent.

1 Like
3

Here are general formulas

#define CONVERSIONG 3.9  // convert the raw readings to the g unit (1g = 9.8 m/s²) depends on sensor settings

accelerationX = (int16_t)(ax * CONVERSIONG);
accelerationY = (int16_t)(ay * CONVERSIONG);
accelerationZ = (int16_t)(az * CONVERSIONG);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;
yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

You will need to import the math.h library (where M_PI is defined) if you have not done so already

There is also this excellent article with an arduino example

4

I modified the code, so it takes timing between measurements into account, and gives me how much has the board been rotated since the last measurement in degrees.

But aren't the formulas you gave me are for an accelerometer?

5

when you do

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

what you get back is raw 6-axis motion sensor readings for acceleration on 3 axis and gyro on 3 axis.

from that you can derive pitch, roll and yaw

not sure what you are trying to do

6

If you just integrate gyro values, you get what seems to be yaw pitch and roll, but it is not. If you pitch up, then yaw, arduino will think it is pitched up, but while due to yaw, it will be rolled, and not pitched up. However, i'm only thinking about doing basic stabilization (i.e. resisting various angular movements), so I might not need some complicated integration after all.

7

I have this code lying around for my testing of my MPU6050 I have it documented well and it should work nicely
This should save you time if you want the actual angles. and it also resolves issues I faced with other examples floating around out there

Wiring Note pin 2 must be attached to int on the mpu6050

900×545 81.1 KB

MPU6050_Test_Code.ino (9.45 KB)

8

zhomeslice:
I have this code lying around for my testing of my MPU6050 I have it documented well and it should work nicely
This should save you time if you want the actual angles. and it also resolves issues I faced with other examples floating around out there

Wiring Note pin 2 must be attached to int on the mpu6050

900×545 81.1 KB

Hello,

In your code MPU6050_Test_Code.ino you wrote :

// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino <<< download to calibrate your MPU6050 put the values the probram returns below

but where can I download MPU6050_calibration.ino ?

Thank you

9

mybrain_iq55:
Hello,

In your code MPU6050_Test_Code.ino you wrote :

// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino <<< download to calibrate your MPU6050 put the values the probram returns below

but where can I download MPU6050_calibration.ino ?

Thank you

Welcome
Z

MPU6050_calibration.ino (7.64 KB)