From 8f746318820bd65beb46de6114579303f847ad61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Dec 2011 20:41:25 +1100 Subject: [PATCH] DCM: don't use compass unless its healthy --- libraries/AP_DCM/AP_DCM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 513001b0d6..ceedb1055f 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -309,7 +309,7 @@ AP_DCM::drift_correction(void) //*****YAW*************** - if (_compass) { + if (_compass && _compass->healthy) { // We make the gyro YAW drift correction based on compass magnetic heading error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error