Angular position is the summation of angular speed
Say your friend invites you to his new house out in the country. His instructions are to take a certain exit off the highway, travel 4 miles and take a right. Your odometer stopped working a while back, but you know if you travel at 60 miles per hour that you cover about a mile every minute. After 4 minutes, you find your right hand turn...
Digital gyrometers give us angular speed, not angular position. We can get an idea of our position by taking a look at our angular speed every so often, and keeping track of how fast we were rotating during that time. We use something like this:
GyroRateXaxis *= (time/1000);
Pitch += GyroRateXaxis;
delay(time);
Your Arduino can now have a rough idea of your angular position as you move it around. It is accurate over a short term, but after a while, errors start to add up. You may notice that your gyro will output a positive or negative speed even when left still on the desk. If this "noise" was in the range of + 0.03 and the gyro usually reports 0.02, we could write something like this to prevent it from building up too much in our angular position sum:
if ( GyroRateXaxis < 0.03  GryoRateXaxis > 0.03 )
{
GyroRateXaxis += 0.02;
GyroRateXaxis *= (time/1000);
Pitch += GyroRateXaxis;
}
delay(time);
The angular position will stay accurate for longer, but for it to truly be useful, we need to take some data from an accelerometer and add it into our angular position. Getting an angle from an accelerometer looks something like this;
accelXangle = atan2(AccelYvalue, AccelZ value);
Notice that data from the accelerometer is not a speed, and doesn't have to be integrated over time like the gyro.
We don't want to rely on it very much though, as there are many situations where the accelerometer will give poor data. Any acceleration that it measures that isn't from gravity will make our angular position inaccurate. Therefore, since the accelerometer is periodically very inaccurate but over the long run always knows where down is, we want to fold small bits of information from it into our angular position calculations. This will counteract the error buildup from the gyro integration.
Digital gyrometers give us angular speed, not angular position. We can get an idea of our position by taking a look at our angular speed every so often, and keeping track of how fast we were rotating during that time. We use something like this:
GyroRateXaxis *= (time/1000);
Pitch += GyroRateXaxis;
delay(time);
Your Arduino can now have a rough idea of your angular position as you move it around. It is accurate over a short term, but after a while, errors start to add up. You may notice that your gyro will output a positive or negative speed even when left still on the desk. If this "noise" was in the range of + 0.03 and the gyro usually reports 0.02, we could write something like this to prevent it from building up too much in our angular position sum:
if ( GyroRateXaxis < 0.03  GryoRateXaxis > 0.03 )
{
GyroRateXaxis += 0.02;
GyroRateXaxis *= (time/1000);
Pitch += GyroRateXaxis;
}
delay(time);
The angular position will stay accurate for longer, but for it to truly be useful, we need to take some data from an accelerometer and add it into our angular position. Getting an angle from an accelerometer looks something like this;
accelXangle = atan2(AccelYvalue, AccelZ value);
Notice that data from the accelerometer is not a speed, and doesn't have to be integrated over time like the gyro.
We don't want to rely on it very much though, as there are many situations where the accelerometer will give poor data. Any acceleration that it measures that isn't from gravity will make our angular position inaccurate. Therefore, since the accelerometer is periodically very inaccurate but over the long run always knows where down is, we want to fold small bits of information from it into our angular position calculations. This will counteract the error buildup from the gyro integration.
AccelXangle = atan2(AccelYvalue, AccelZ value); AccelXangle*= RAD_TO_DEG; AccelXcomp=(PitchAccelXangle); AccelXcomp*=0.001; Pitch=AccelXcomp if ( GyroRateXaxis < 0.03  GryoRateXaxis > 0.03 ) { GyroRateXaxis += 0.02; GyroRateXaxis *= (time/1000); Pitch += GyroRateXaxis; } delay(time); 

Keeping track of Pitch and Roll as Yaw changes

There is one major weakness in the above commonly used code: It does not take into account that pitch and roll change as yaw changes. In fact, when you yaw 90 degrees, your new pitch equals your previous roll, and vis versa. at 180 degrees, your pitch is now opposite of what it previously was. .

. You could wait a few seconds for the accelerometer input to slowly pull the pitch and roll to the correct values, but your aircraft could also stall and crash during those few seconds!
I'm not the first person to solve this problem by any means, but I couldn't find any info on how to do it. After two weeks, a dozen failed algorithms, and 6 months worth of hair loss, I found something that worked. The way that I solved this problem is by changing the way that i store the aircraft's position: instead of keeping pitch and roll separate, I combined them into one angle. 

Visualize your aircraft as a plate laying flat on a table. By lifting the front of the plate, you increase it's pitch. Lifting a side increases roll. Lifting both at the same time creates a high point between the two. This high point has an angle, and it also has a magnitude.
Its a vector. The vector, I dub Theta, from the previous calcuation is combined with the latest movement about the Z axis, then converted to cartesian components. The new X and Y axis rotations are added to the two theta components, and the 360 degree rollover statements are applied. Theta then is converted back into a vector.Pitch and roll are then defined from sine and cosine of theta, multiplied by theta's magnitude, I dub T. 
Even without an accelerometer, this method will accurately track angular position much longer than simply integrating pitch and roll from the X and Y angular speeds that the gyro provides.
The code looks something like this;
flgx+=1.00; //median calibration. will be different for every gyro
flgy+=0.60;
flgz+=0.40;
flgx*=(time / 1000.00);
flgy*=(time / 1000.00);
flgz*=(time / 1000.00);
if (flgz<0.03flgz>0.03){ //changes in yaw directly change theta
theta *= RAD_TO_DEG;
theta = flgz; //todo: integrate future magnetometer data here
theta *= DEG_TO_RAD;}
if (flgz<0.03flgz>0.03){yaw += flgz;}
if (yaw>360)yaw=360;
if (yaw<0)yaw+=360;
thetaXcomp = t*sin(theta);
thetaYcomp = t*cos(theta); //break theta and t into x and y components
if (flgx<0.03flgx>0.03){thetaXcomp += flgx;}
if (flgy<0.03flgy>0.03){thetaYcomp += flgy;}
abflaz=abs(flaz);
accelXangle = atan2(flay, abflaz); //prepared accel data injected here
accelXangle*= RAD_TO_DEG; //future magnetometer should be integrated in the same fashion
accelXcomp=(thetaXcompaccelXangle);
accelXcomp*=0.001; //accel data weighting factor
thetaXcomp=accelXcomp;
accelYangle = (atan2(flax, flaz));
accelYangle*= RAD_TO_DEG;
accelYcomp=(thetaYcompaccelYangle);
accelYcomp*=0.001; //accel data weighting factor
thetaYcomp=accelYcomp;
if(thetaXcomp<180){thetaXcomp+=360;} //360 degree rollover
if(thetaXcomp>180){thetaXcomp=360;}
if(thetaYcomp<180){thetaYcomp+=360;}
if(thetaYcomp>180){thetaYcomp=360;}
t = sqrt((sq(thetaXcomp)+sq(thetaYcomp))); //convert theta back to polar
theta = atan2(thetaXcomp, thetaYcomp);
pitch = t*sin(theta); //define pitch and roll from theta
roll = t*cos(theta);
if (digitalRead(switchpin) == LOW) //reset button
{pitch = 0;
roll = 0;
yaw = 0;
t = 0.01;
theta = 0;}
This file contains code written for Arduino, an MPU6050 gyro, and a 16x2 lcd display, but won't be hard to adapt to other gyros.
Hope you enjoy! Please forgive any blatant errors, I'm new at this.
thanks,
Michael
Hope you enjoy! Please forgive any blatant errors, I'm new at this.
thanks,
Michael