diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 3576d252f3..2cc3b6e827 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -325,7 +325,10 @@ AP_AHRS_DCM::drift_correction_yaw(void) if (_compass->last_update != _compass_last_update) { yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; _compass_last_update = _compass->last_update; - if (!_have_initial_yaw) { + // we force an additional compass read() + // here. This has the effect of throwing away + // the first compass value, which can be bad + if (!_have_initial_yaw && _compass->read()) { float heading = _compass->calculate_heading(_dcm_matrix); _dcm_matrix.from_euler(roll, pitch, heading); _omega_yaw_P.zero();