# 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?

• I'm a little confused by the last statement - if you were only expecting a rotational change on Y, than nothing happening is right as the accelerometer measures linear acceleration. Also note on that chip that the coordinate system for the accelerometer and the gyro are different Dec 8 '14 at 15:32
• I was under the impression that using these equations I'd be able to calculate the angles, not just the accelerations? I haven't seen anywhere that the coordinate systems are different - would it be possible to show an example? Dec 8 '14 at 16:11
• When I said the coordinate system is different - I meant they still both use x,y,z coordinates, its just the directions they point to are different. Dec 8 '14 at 16:17
• How much different are they? Dec 8 '14 at 20:27

Try this:

arx = (180/3.141592) * atan(ax / sqrt(square(ay, 2) + square(az, 2)));
ary = (180/3.141592) * atan(ay / sqrt(square(ax, 2) + square(az, 2)));
arz = (180/3.141592) * atan(sqrt(square(ay) + square(ax)) / az);


nw see you arx and ary.

Also change

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


to:

rx = (0.96 * arx) + (0.04 * grx);
ry = (0.96 * ary) + (0.04 * gry);
rz = (0.96 * arz) + (0.04 * grz);