Arrange code of QCopter_ahrs.c