The gyro reading will drift over time. This is okay as we only need to trust once every loop.
The output from the complementary filter(CFangleX)Shouldn’t drift as it trusts the accelerometer a lot more than it trusts the gyro.
Try playing around with this value to see if it improves
#define AA 0.97 // complementary filter constant
Mark --OzzMaker.com --
