Add AHRS code to UnoRTOS