I wrote some code that utilizes an ADXL345 accelerometer and a ITG3205 gyroscope to determine the angle relative to the horizon of my quadcopter. When the quad is sitting still, all is good. If I move it about somewhat roughly, the pitch angle will settle back to close to zero, as expected. But for some reason, the roll angle always ends up at a much higher value, anywhere from 9 to 90 degrees when it is again level. The code is the same in determining each angle respectively, so I am assuming that it is somehow accumulated drift from the gyroscope.
Any ideas? Help would be greatly appreciated. The code snippet doing these calculations is listed below.
void updateSensors(){
getAccel();
getGyro();
getMag();
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];
Serial.print(" pitchAngle "); Serial.print(accAngle[0]); Serial.print(" rollAngle "); Serial.print(calcAngle[1]);
}
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_temp = (((((int)buffGyro[2]) << 8) | buffGyro[3]) & 65528) + g_offx; //Shifting the first byte 8 bits to the left and then concatenating the first and second bytes using a bit-wise OR operator ------ 65532
gyroy_temp = (((((int)buffGyro[4]) << 8) | buffGyro[5]) & 65528) + g_offy; //the concatenated values are then bit-wise ANDed with a binary value of "1111111111111100" that effectively turns the last 2 bits to zero
gyroz_temp = (((((int)buffGyro[6]) << 8) | buffGyro[7]) & 65528) + g_offz; //so that the noise can be ignored
//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)gyrox_temp*dt/14.375; //Per the data sheet, sensitivity scale factor per LSB/(deg/sec)
gyroY = (double)gyroy_temp*dt/14.375;
gyroZ = (double)gyroz_temp*dt/14.375;
gyroMAF();
gyroAngle[0] += gyroX_ave;
gyroAngle[1] += gyroY_ave;
gyroAngle[2] += gyroZ_ave;
gyroMAFCounter++;
if (gyroMAFCounter == (GYRO_MAF_NR - 1)) gyroMAFCounter = 0;
ntg = micros();
}
Serial.print(" gyrox_temp "); Serial.print(gyrox_temp);Serial.print(" gyroy_temp "); Serial.println(gyroy_temp);
}