Create Debug port function for ahrs task work frequency measurement
Enable Timer2 (Frequency = 500hz)
Use timer to ensure the AHRS task work frequency (Not finished yet)