Gyro integration highly inaccurate quickly

So I finally have my quadcopter flying (amazingly, with how quickly my angles go wrong), but to go any further I know I need to fix this problem. I have the GY-85 IMU, including the ITG-3205 gyro, ADXL-345 accel. I have tried changing the sample rate, the low pass filter on the gyro, and even ignore the 2 LSB's of the gyro data, an idea I got from looking at multiwii.

But my problem remains, namely that it will give good readings when at rest (as long as i calibrate it), and fairly good readings for a little while as i move it gently. But if i move it in odd directions (never more than 60 deg from the horizontal), similar to what it feels when flying, the integrated values from the gyroscope become drastically wrong fairly fast. I will often set it down and see values between 15 and 120 deg for pitch and roll. I have checked that it is solely the integrated readings from the gyro that are really far off.

I have included the code I use to get the sensor values, and i apologize if its fairly basic as i have no programming background. But any input on how I might correct this, whether its in my algorithms or otherwise, would be much appreciated!

void updateSensors(){
  calcAngle[0] = .90*gyroAngle[0]+(1-.90)*accAngle[0];                    //Complementary filter for calculating angle from both gyro and acc data
  calcAngle[1] = .90*gyroAngle[1]+(1-.90)*accAngle[1];

void sensorInit(){
  writeTo(accelDev,0x2D, 0);                                             //Writes to different sensor registers to set power modes, filters, etc... see data sheets for ADXL345, ITG3205, HMC5883L
  writeTo(accelDev, 0x2D, 16);
  writeTo(accelDev, 0x2D, 8);
  writeTo(gyroDev, 0x3E, 0x81);                                           //Sets CLK_SEL=1, enabling PLL with X Gyro reference for clock source, sets H_RESET=1, resetting device to default settings
  writeTo(gyroDev, 0x15, 0x09);                                          //Sets the sampling rate in SMPLRT_DIV=9, and gives F=1khz/(9+1) = 100 Hz, or 10 ms per sample
  writeTo(gyroDev, 0x16, 0x1D);                                          //Sets FS_SEL=3, which gives the max range of +/- 2000 deg/s. DLPF_CFG = 5, sets LPF Bandwidth to 10 Hz and Internal Sample Rate to 1 kHz
  writeTo(magDev, 0x02, 0x00);

void getAccel(){
  if ((micros() - nta) >= 2500){                                         //Only update every 2500 microseconds, ~400 Hz rate
    int regAddress = 0x32;
    readFrom(accelDev, regAddress, 6, buffAcc);                          //Starting at the first data register, reades the bytes for x, y, z axis successively
    accX = ((((int)buffAcc[1]) << 8) | buffAcc[0]) + acc_offx;           //Shifts the first byte left 8 places and concatenates first byte with second to get 16-bit sensor value
    accY = ((((int)buffAcc[3]) << 8) | buffAcc[2]) + acc_offy;
    accZ = ((((int)buffAcc[5]) << 8) | buffAcc[4]) + acc_offz;
    faccX = accX * alpha + (faccX * (1.0 - alpha));                       //Low Pass Filter
    faccY = accY * alpha + (faccY * (1.0 - alpha));
    faccZ = accZ * alpha + (faccZ * (1.0 - alpha));
    accAngle[1] = -(atan2(faccX, faccZ))*radToDeg;                         //Y-axis angle, Roll
    accAngle[0] = (atan2(faccY, faccZ))*radToDeg;                          //X-axis angle, Pitch
    nta = micros();

void getGyro(){
  if ((micros() - ntg) >= 10000){                                                  //Only update every 10,000 microseconds, ~100 Hz rate
    int regAddress = 0x1B;
    readFrom(gyroDev, regAddress, 8, buffGyro);
    gyrox_temp0 = ((((int)buffGyro[2]) << 8) | buffGyro[3]);    //Shifting the first byte 8 bits to the left and then concatenating the first and second bytes using a bit-wise OR operator
    gyroy_temp0 = ((((int)buffGyro[4]) << 8) | buffGyro[5]);    //the concatenated values are then bit-wise ANDed with a binary value of "1111111111111100" (65532) that effectively turns the last 2 bits to zero
    gyroz_temp0 = ((((int)buffGyro[6]) << 8) | buffGyro[7]);    //so that the noise can be ignored
    gyrox_temp1 = ((gyrox_temp0 + g_offx) & 65532);
    gyroy_temp1 = ((gyroy_temp0 + g_offy) & 65532);
    gyroz_temp1 = ((gyroz_temp0 + g_offz) & 65532);
    gyroXRate = (double)(gyrox_temp1)/14.375;
    gyroYRate = (double)(gyroy_temp1)/14.375;
    gyroZRate = (double)(gyroz_temp1)/14.375;
    //temp = (((int)buffGyro[0]) << 8) | buffGyro[1];                             //Reading the data from the ITG3205 for temperature, not currently used
    double dt = (micros() - ntg)/1000000.00;                                      //To calculate the time since the last measurement
    gyroX = (double)gyroXRate*dt;                                         //Per the data sheet, sensitivity scale factor per LSB/(deg/sec) 
    gyroY = (double)gyroYRate*dt;
    gyroZ = (double)gyroZRate*dt;
    gyroAngle[0] += gyroX_ave;
    gyroAngle[1] += gyroY_ave;
    gyroAngle[2] += gyroZ_ave;
    if (gyroMAFCounter == (GYRO_MAF_NR - 1)) gyroMAFCounter = 0;
    ntg = micros();


void gyroMAF(){
  gyroX_MAF[gyroMAFCounter] = gyroX;
  gyroY_MAF[gyroMAFCounter] = gyroY;
  gyroZ_MAF[gyroMAFCounter] = gyroZ;
  for (int i = 0; i < GYRO_MAF_NR; i ++){
    gyroX_ave += gyroX_MAF[i]; 
    gyroY_ave += gyroY_MAF[i];
    gyroZ_ave += gyroZ_MAF[i];
  gyroX_ave = gyroX_ave / GYRO_MAF_NR;
  gyroY_ave = gyroY_ave / GYRO_MAF_NR;
  gyroZ_ave = gyroZ_ave / GYRO_MAF_NR;

You haven't told us anything about the algorithm in your code. Where did it come from, and do you have any reason to believe that it should work?

A fixed reference system is required to correct gyro drift on all axes, and a direction other than down is required. I am not certain that "pitch" and "roll" have any meaning when you don't know which direction X and Y are pointing. A magnetometer is usually used to indicate magnetic North and when combined with the accelerometer data, provides that stable reference.

Finally, have you calibrated the accelerometer?

The algorithm that I am using, specifically the one to calculate the angle based on gyros rate of change over time, is I believe the only way to determine the angle in that case. How else can you determine an angle from an angular rate other than integrating? I have seen its like in many other codes, and none seemed to have this problem.

I also know that it is possible to get an accurate angle without a third sensor, whether its a magnetometer or GPS unit. I have used the FreeIMU and FreeSixIMU libraries to give me much more stable angles, but my aim here is to figure out how to code it on my own. I've made it this far with no experience, I see no reason to stop yet.

As far as calibration for the accelerometer, do you mean anything other than setting it's offsets and choosing its internal register settings? Because I have done that. Regardless, it is the gyro I seem to be having issues with.

All gyros drift. It is the algorithm to compensate for that drift that is causing you difficulty, and you have presented experimental evidence that could lead one to conclude that it does not work.

For the most accurate measurements, the gain on each axis of an accelerometer usually needs to be adjusted as well as the offset. My favorite procedure (designed for magnetometers, but works equally well for accelerometers if the sensor is held still while each data point is collected) is Sailboat Instruments: Improved magnetometer calibration (Part 1)

Then have you ever worked with code that utilized an IMU to get accurate yaw/pitch/roll readings? It looks like the use of quaternions, which are used in some code, might offer better stability. Or do you believe that there is a way to use the accelerometer, while in flight, to correct for this drift? The more I think about it, the less possible it seems. There will always be some acceleration force acting on the quad other than gravity, so simply setting the gyro angles back to 0 when the orientation is thought to be stable would not seem to work.

What you want (to study, if not to use) is an open source AHRS (Attitude, Heading and Reference System), which uses a magnetometer in addition to an accelerometer to correct gyro drift. An AHRS can also use the gyro to correct for errors in the accelerometer data, which crop up during rapid rotation.

There are several well thought out examples available for the Arduino, with the most popular being Ardupilot (APM:Copter) and somewhere, there is a comprehensive document describing the algorithms in use. Another AHRS currently under development is RTImulib Last but not least is X-Imu

Quaternions have a couple of advantages over other methods (like Euler angles) of representing the current orientation of the craft, but with them the code becomes somewhat more complicated and less easy to understand.

Thanks for the pointers. I guess I'll have to spend my time studying those now, to bad school is starting up and I will have no life. Would certainly be fun to have written my own AHRS code, but I am also realizing that having no programming background makes this more difficult.

Again, I appreciate the responses!

Here is the paper describing the original DCM algorithm in Ardupilot. It is clearly written and goes into some detail about how to handle the gyros:

In particular, you should look at the section titled "Drift Cancellation" (p. 17) which discusses how to use an external reference to reduce or cancel gyro drift. There is nothing in your code that does that (and without a second direction reference, it is not possible).