Fixed the conflict with servo and serial AHRS. Timer code is commented out. Not tested since I am unable to print floats to serial now.