From 5184bca87ffd12ec12538a341fb96c556d8cbb41 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 22 Apr 2015 11:15:48 +1000
Subject: [PATCH] AP_AHRS: Prevent DCM reporting good tilt error when inverted

---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index b1022dbeec..e1fb05f964 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -647,6 +647,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // running at 800Hz and the MPU6000 running at 1kHz, by combining
     // the two the effects of aliasing are greatly reduced.
     Vector3f error[INS_MAX_INSTANCES];
+    float error_dirn[INS_MAX_INSTANCES];
     Vector3f GA_b[INS_MAX_INSTANCES];
     int8_t besti = -1;
     float best_error = 0;
@@ -673,11 +674,18 @@ AP_AHRS_DCM::drift_correction(float deltat)
             continue;
         }
         error[i] = GA_b[i] % GA_e;
+        // Take dot product to catch case vectors are opposite sign and parallel
+        error_dirn[i] = GA_b[i] * GA_e;
         float error_length = error[i].length();
         if (besti == -1 || error_length < best_error) {
             besti = i;
             best_error = error_length;
         }
+        // Catch case where orientation is 180 degrees out
+        if (error_dirn[besti] < 0.0f) {
+            best_error = 1.0f;
+        }
+
     }
 
     if (besti == -1) {