From 01ae8f8771fc8d982272ecdac7d9d68b59a10cf0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 18 Aug 2012 13:53:59 +1000
Subject: [PATCH] AHRS: check for bad values in the error before they can
 affect DCM

this should fix the DCM renorm errors in autotest, probably caused by
bad climb rates
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 2cc3b6e827..c3f3a1ed73 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -553,6 +553,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // convert the error term to body frame
     error = _dcm_matrix.mul_transpose(error);
 
+    if (error.is_nan() || error.is_inf()) {
+	    // don't allow bad values
+	    check_matrix();
+	    return;
+    }
+
     _error_rp_sum += error.length();
     _error_rp_count++;