From 0424b3f93ca3972d2559187d8ef8223c4d7f85f2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 26 Aug 2015 13:57:54 +0900
Subject: [PATCH] Copter: pre-arm check of EKF compass variance

---
 ArduCopter/motors.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp
index 2c2526799c..afea44d10a 100644
--- a/ArduCopter/motors.cpp
+++ b/ArduCopter/motors.cpp
@@ -652,6 +652,18 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
         return false;
     }
 
+    // check EKF compass variance is below failsafe threshold
+    float vel_variance, pos_variance, hgt_variance, tas_variance;
+    Vector3f mag_variance;
+    Vector2f offset;
+    ahrs.get_NavEKF().getVariances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
+    if (mag_variance.length() >= g.fs_ekf_thresh) {
+        if (display_failure) {
+            gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: EKF compass variance"));
+        }
+        return false;
+    }
+
     // check home and EKF origin are not too far
     if (far_from_EKF_origin(ahrs.get_home())) {
         if (display_failure) {