Gy512 - calculate current angle. Long warning!

Gday - Thanks for reading my “HELP!”

I’m trying to use the Gy512 6dof. trying to work out the current angle
I have some questions. (I’m trying to build up to a kalman filter for… fun)

1 - The accelerometer
measures a change in angle since the last time you got data?
is only accurate in the short term, as you add the acc data together it drifts… presumably because of compunding rounding errors.
to get the rate of change per second you have to consider how many times per secod you are poling the acc… That’s the “DELTA T”

2 - The gyroscope
measures the angle it is at time of ‘get data’?
This data while is accurate is subject to short term errors.

the ‘angle’ it gives out seem to be between +/- 32727
using map(data,-32767,32767,0,360) ; will convert the data to an angle (or for the acc to work… you probably want -180,180 as the output range)

3 - the complementry filter (which doent work for me)

Filter Angle = .95 ( gyr data) + .5 (acc angle) ← the acc angle needs to remember the Delta T

if you use this method… wont the filtered angle drift over time (which mine seems to)…

I haven’t included code I’m trying to get my ‘overview correct’ I must be missing something really obvious!

That's why a software filter is needed. The gyro drifts and the accelerometer is noisy. Start with the Kalman Guide : http://forum.arduino.cc/index.php?topic=58048.0 The i2cdevlib with the MPU-6050 uploads firmware onto the MPU-6050 so the sensor itself can do some calculations : https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino (the http://www.i2cdevlib.com seems to be down at the moment).

oh oh oh … I think I figured it out! ← insert hands being up in the air!

I’m so going to do a share thing with this.

Here is my code… I was all kinds of wrong. close… but so far! I think my complementary filter is reasonable for a first timer.

any suggestions anyone?


unsigned long CurrentTime,LastTime,DeltaT ;

float Acc_angle_x = 0;
float Gyr_angle_x = 0 ;
float Current_Gyr_angle_x = 0;
float ComplementryAngle = 0 ;

int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050

void setup(){
Wire.begin();
Serial.begin(9600) ;
Serial.println ("_______________________________________________") ;

Serial.print (“dummy”); Serial.print (",") ; Serial.print (“Acc_angle_x” ) ; Serial.print (",") ;Serial.print (“Current_Gyr_angle_x” ) ; Serial.print (",") ;Serial.print (“ComplementryAngle” ) ;

Serial.println () ;
Serial.println ("") ;
DeltaT = 100 ;// Deltat should not be 0 to start with. 100 is a close enough for starts.
}

void loop(){
LastTime= millis();

Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

Acc_angle_x = map(AcX, -32767, 32767, -180, 180); //ok
Gyr_angle_x = map(GyX, -32767, 32767, -180, 180); //ok
Current_Gyr_angle_x += Gyr_angle_x ;
ComplementryAngle = (.98*(ComplementryAngle + Gyr_angle_x /DeltaT)) + (.02 *(Acc_angle_x )) ;

CurrentTime = millis();
DeltaT = CurrentTime - LastTime ;

Serial.print (“dummy”); Serial.print (",") ; Serial.print (Acc_angle_x ) ; Serial.print (",") ;Serial.print (Current_Gyr_angle_x ) ; Serial.print (",") ;Serial.print (ComplementryAngle ) ;
Serial.println () ;
}