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(){
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];
}
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
delay(50);
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;
gyroMAF();
gyroAngle[0] += gyroX_ave;
gyroAngle[1] += gyroY_ave;
gyroAngle[2] += gyroZ_ave;
gyroMAFCounter++;
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;
}