Calculating Angles from MPU6050

I've been playing with a GY521 breakout recently with the MPU6050 chip on it, though i've hit a bit of a problem that I can't quite work out

I know when I rotated the board I rotated ~90 degrees about the Y axis, and using the sensitivity numbers from the datasheet (131) and integrating i've managed to get something that looks believable from the gyroscope, but am having problems with the accelerometer.

From a number of sources, this being one of them, I've managed to get the three equations seen on here, however when I apply them to either the raw or scaled data (as they're scalers is shouldn't make a difference?) I get a set of data that varies from 0 to 90, but only on the X and Z axes, while nothing happens to the Y.

Updated to include code:

``````#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 accelgyro;

int16_t ax, ay, az, gx, gy, gz;

double timeStep, time, timePrev;
double arx, ary, arz, grx, gry, grz, gsx, gsy, gsz, rx, ry, rz;

int i;
double gyroScale = 131;

void setup() {

Wire.begin();
Serial.begin(9600);
accelgyro.initialize();

time = millis();

i = 1;

}

void loop() {

// set up time for integration
timePrev = time;
time = millis();
timeStep = (time - timePrev) / 1000; // time-step in s

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

// apply gyro scale from datasheet
gsx = gx/gyroScale;   gsy = gy/gyroScale;   gsz = gz/gyroScale;

// calculate accelerometer angles
arx = (180/3.141592) * atan(ax / sqrt(square(ay) + square(az)));
ary = (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));
arz = (180/3.141592) * atan(sqrt(square(ay) + square(ax)) / az);

// set initial values equal to accel values
if (i == 1) {
grx = arx;
gry = ary;
grz = arz;
}
// integrate to find the gyro angle
else{
grx = grx + (timeStep * gsx);
gry = gry + (timeStep * gsy);
grz = grz + (timeStep * gsz);
}

// apply filter
rx = (0.1 * arx) + (0.9 * grx);
ry = (0.1 * ary) + (0.9 * gry);
rz = (0.1 * arz) + (0.9 * grz);

// print result
Serial.print(i);   Serial.print("\t");
Serial.print(timePrev);   Serial.print("\t");
Serial.print(time);   Serial.print("\t");
Serial.print(timeStep, 5);   Serial.print("\t\t");
Serial.print(ax);   Serial.print("\t");
Serial.print(ay);   Serial.print("\t");
Serial.print(az);   Serial.print("\t\t");
Serial.print(gx);   Serial.print("\t");
Serial.print(gy);   Serial.print("\t");
Serial.print(gz);   Serial.print("\t\t");
Serial.print(arx);   Serial.print("\t");
Serial.print(ary);   Serial.print("\t");
Serial.print(arz);   Serial.print("\t\t");
Serial.print(grx);   Serial.print("\t");
Serial.print(gry);   Serial.print("\t");
Serial.print(grz);   Serial.print("\t\t");
Serial.print(rx);   Serial.print("\t");
Serial.print(ry);   Serial.print("\t");
Serial.println(rz);

i = i + 1;
delay(50);

}
``````

Results:

Strikes me as a little odd, as I was expecting only a rotational change in Y. Any suggestions?

• could you post those code samples? – BrettAM Dec 8 '14 at 17:51

If I understand your question, the main issues are that the axis don't match up, and z from the accel is not accurate?

with respect to the axis: just swap the values. Rotation math is self-consistent and "correct" in many different permutations. What makes it correct is the set of equations, not any single equation; So, copying equations from one reference frame to another will yield incorrect results even if they were correct originally.

"Interpreting the diagram at left is a little tricky, since the tilt of the X-axis actually shows rotation around the Y-axis, and the angling of the Y-axis shows (negative) rotation around the X-axis."

``````  ary = -(180/3.141592) * atan(ax / sqrt(square(ay) + square(az)));
arx =  (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));
``````

should work better for you

On the second point: accelerometers can't measure yaw, the "z" equation can safely be ignored. that isn't yaw it calculates. Calculating z from the gyro alone should fix your second issue.

Other considerations:

• The gyroscope tracking can be made more accurate by using the midpoint rule in your integration
• Your code is not integrating the gyroscope's signal with the best estimate of its last position (rx, ry, rz), instead it integrates the gyro with separate numbers (grx, gry, grz). In effect information from the accelerometer is discarded each loop and gyro drift is not corrected for.
• Thanks for including those graphs with your code samples =] rotation math is extremely tricky, and graphs help a lot
• Thanks BrettM! I originally had the gyroscope using the rx, ry and rz values but changed it at some point while playing with the accelerometer numbers, thanks for reminding me to change it back, I'd forgotten. I was also planning to update to using the midpoint, but had shortened it being a bit lazy. Thanks again for helping me sort out my original question! – Laurengineer Dec 9 '14 at 6:57