Add 'AHRS_and_RC_updata()' function to reduce code in the main.c