Copter: pre-arm check of EKF compass variance

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) {