Hi all,
I'm building a drone and have an IMU loop that runs at 500Hz. The loop is simple and calculates the X and Y angle relative to the horizon using a complementary filter. I hooked up all the hardware (dual axis thrust vectored motor) and noticed that in the X axis it is fine, but in the Y axis there is a lag when rotating the vehicle and the thrust vectoring is 'catching up'. I went through all the PID loops and found them to be working correctly, so I focussed my attention on the IMU loop. Bit by bit I checked each variable and have come to a dead end.
void GetIMU(){
uint32_t IMUStartT = micros();
uint16_t IMUdT = IMUStartT - IMUFinT; //dT for each computation of the IMU loop
if(IMUdT >= IMUlooptime){
Wire.beginTransmission(MPU_Address);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,4);
Accel_X = Wire.read()<<8 | Wire.read();
Accel_Y = Wire.read()<<8 | Wire.read();
Wire.beginTransmission(MPU_Address);
Wire.write(0x3F);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,4);
Accel_Z = Wire.read()<<8 | Wire.read();
Temp = Wire.read()<<8 | Wire.read();
Wire.beginTransmission(MPU_Address);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(MPU_Address,6);
Gyro_X = Wire.read()<<8 | Wire.read();
Gyro_Y = Wire.read()<<8 | Wire.read();
Gyro_Z = Wire.read()<<8 | Wire.read();
Accel_X = Accel_X - Accel_X_offset;
Accel_Y = Accel_Y - Accel_Y_offset;
Accel_Z = Accel_Z - Accel_Z_offset;
Accel_X_Filter = Accel_X_Filter + filter*(Accel_X - Accel_X_Filter);
Accel_Y_Filter = Accel_Y_Filter + filter*(Accel_Y - Accel_Y_Filter);
Accel_Z_Filter = Accel_Z_Filter + filter*(Accel_Z - Accel_Z_Filter);
Acc_Angle_X = atan2(Accel_Y_Filter, sqrt( Accel_X_Filter*Accel_X_Filter + Accel_Z_Filter*Accel_Z_Filter ) )*57.30;
Acc_Angle_Y = atan2(Accel_X_Filter, sqrt( Accel_Y_Filter*Accel_Y_Filter + Accel_Z_Filter*Accel_Z_Filter ) )*57.30;
//Fine up to here,
IMUdt_Const = (IMUdT*0.0000000152579); //65LSB/d/s, micros, 1/65540000= 0.00001525..
Xdelta = compangleX + (Gyro_X*IMUdt_Const); // calculates first bit of comp. filter eq.
Ydelta = compangleY + (Gyro_Y*IMUdt_Const); // calculates first bit of comp. filter eq.
//Ydelta is delayed...
compangleX = 0.98*Xdelta + 0.02*Acc_Angle_X;
compangleY = 0.98*Ydelta + 0.02*Acc_Angle_Y;
Serial.println(compangleX);
YawRate = (-Gyro_Z/65.54);
IMUFinT = IMUStartT; // leave in IF loop, then only computed when If=true.
}
}
Basically compangleX is behaving as expected, but compangleY has this inherent delay. In the attached photo "gain=0.98 Y" you can see the compangleY output over time as I manually rotate the IMU chip. There is an initial portion of the curve where the value goes the wrong way, this is what I think is causing the delay as it is having to catch up with itself.
I checked each part or the complementary filter individually, the gyro part and the accelerometer part and found no such delay in either. They both respond quickly. Its only when they are both added in the filter this strange behaviour is exhibited. In photo "Gyro Only Y" there is no delay and this value comes straight from the gyro.
In photo "Y accel only", again there is no delay and this is coming straight from the accelerometers.
It is only when these two elements are summed in the complementary filter this happens, but it only happens in Y, the X value is unaffected.
Ive changed the MPU chips over and the same result was observed. All values are declared as floats, apart from the raw data from the IMU which are int16_t.
If anyone has any ideas I'd love to hear them as I've spent about 2 days trying to figure this one out and it is now beyond me!
Thanks.
