Refactored and DRY'd out the ahrs_init code