Corrected version for AHRS Version (Arduino code)