From eb35e0e8acf6c7ba310eeba49893a494d0f4117f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 26 Feb 2012 22:23:29 +1100 Subject: [PATCH] DCM: fixed the sense of the compass GPS test in initial yaw we were only disabling null offsets when we didn't have a compass, which doesn't make much sense! --- libraries/AP_DCM/AP_DCM.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index d82124aa3d..b90e48161b 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -426,11 +426,11 @@ AP_DCM::drift_correction(void) // DCM matrix to the current // roll/pitch values, but with yaw // from the GPS - if (!_compass) { + if (_compass) { _compass->null_offsets_disable(); } rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course)); - if (!_compass) { + if (_compass) { _compass->null_offsets_enable(); } _have_initial_yaw = true;