Hi!
I am using a MPU_6050 for a flight stabilizer. In one mode of operation it is supposed to auto-level the plane by calculating pitch and roll. To do this I opted to use a Complementary Filter for its simplicity over the Kalman Filter.
Now here's my problem:
When I place my circuit as level as I can in a vice so that it can't move my "roll" calculation gives me a value hopping between -1.5 and 1.5 degrees, very stable. Using the exact same function to calculate the pitch gives me an oscillating value between 23 and 29 degrees, not jumping irratically, but transitioning smoothly between the two. So the value is both wrong and non stable.
Is there something I have done wrong here?
void process_MPU(float *pitch, float *roll, float *yaw)
{
float pitchAcc, rollAcc, yawAcc;
//ComplementaryFilter(float *newAngle, float newRate, float accAngle, unsigned long looptime)
long forceMagnitudeApprox = abs(mpu_data.aX) + abs(mpu_data.aY) + abs(mpu_data.aZ);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768){
int16_t aX = mpu_data.aX;
int16_t aY = mpu_data.aY;
int16_t aZ = -mpu_data.aZ;
pitchAcc = atan2(aX, aZ) * 57.2957795;
rollAcc = atan2(aY, aZ) * 57.2957795;
yawAcc = atan2(aX, aY) * 57.2957795;
ComplementaryFilter(pitch, mpu_data.gY, pitchAcc, millis() - lastMPU, true);
ComplementaryFilter(roll, mpu_data.gX, rollAcc, millis() - lastMPU, true);
ComplementaryFilter(yaw, mpu_data.gZ, yawAcc, millis() - lastMPU, true);
}
else {
ComplementaryFilter(pitch, mpu_data.gY, pitchAcc, millis() - lastMPU, false);
ComplementaryFilter(roll, mpu_data.gX, rollAcc, millis() - lastMPU, false);
ComplementaryFilter(yaw, mpu_data.gZ, yawAcc, millis() - lastMPU, false);
}
}
void ComplementaryFilter(float *newAngle, uint16_t newRate, float accAngle, unsigned long looptime, boolean valid){
float dt = looptime / 1000.0;
*newAngle += (float)(newRate / GYROSCOPE_SENSITIVITY) * dt;
if (valid){
*newAngle = (float)((0.95 * *newAngle) + (0.05 * accAngle));
}
}