From 7948b3aee72af4b40b72b83c717a778f0cd705a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 Aug 2012 17:57:39 +1000 Subject: [PATCH] AHRS: force an extra read of the compass on startup the first read from the compass can be bad. This ensures we have a good value when getting the initial AHRS yaw. Thanks to Randy and Jason for the bug report! --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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();