From 13d432968bc9b8bb7255d6a4728697042f160392 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 14 Sep 2016 10:59:58 +1000
Subject: [PATCH] Copter: Prevent localised magnetic anomaly failing pre-arm
 checks

Enables the pre-arm field strength check to pass by lifting the copter away from the source of magnetic interference for 3 seconds
---
 ArduCopter/ArduCopter.pde |  9 +++++++++
 ArduCopter/motors.pde     | 31 +++++++++++++++++++++----------
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 1e09425500..bcd05e78d0 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -658,6 +658,15 @@ static uint32_t last_mag_pass_time_ms = 0;
 // true when the pre-arm compass consistency check has passed for long enough to latch
 static bool mag_pass_latched = false;
 
+// last time in msec that the pre-arm field strength check failed
+static uint32_t last_field_fail_time_ms = 0;
+
+// last time in msec that the pre-arm field strength check passed
+static uint32_t last_field_pass_time_ms = 0;
+
+// true when the pre-arm field strength check has passed for long enough to latch
+static bool field_pass_latched = false;
+
 // Used to exit the roll and pitch auto trim function
 static uint8_t auto_trim_counter;
 
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 4d9491414c..44382d1d68 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -301,23 +301,24 @@ static bool pre_arm_checks(bool display_failure)
             return false;
         }
 
-        // check for unreasonable mag field length
-        float mag_field = compass.get_field().length();
-        if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
-            if (display_failure) {
-                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
-            }
-            return false;
-        }
-
         // Check that the different magnetometers are providing consistent data
+        // Check for unreasonable external magnetic field strength when compasses are consistent
         // Latch the check to passsed if 3 continuous seconds of pass occurs
         // This allows the user to pick up the copter to clear the failure if it is caused by a ground level magnetic anomaly
+        float mag_field = compass.get_field().length();
         if (!compass.consistent()) {
             last_mag_fail_time_ms = millis();
+        } else if (mag_field < COMPASS_MAGFIELD_EXPECTED*1.65 && mag_field > COMPASS_MAGFIELD_EXPECTED*0.35) {
+            // we have consistent compass data and a valid external field strength
+            last_field_pass_time_ms = last_mag_pass_time_ms = millis();
         } else {
-            last_mag_pass_time_ms = millis();
+            // we have an invalid external field strength which cannot be explained by a localised anomaly
+            // this could cause poor yaw accuracy and loss of position control
+            last_field_fail_time_ms = millis();
         }
+
+        // latch the compass consistency check to pass if passed for 3 continuous seconds
+        // this allows the copter to be picked up from the ground to clear the check
         mag_pass_latched = (last_mag_pass_time_ms != 0 && (millis() - last_mag_fail_time_ms) > 3000) || mag_pass_latched;
         if (!mag_pass_latched) {
             if (display_failure) {
@@ -325,6 +326,16 @@ static bool pre_arm_checks(bool display_failure)
             }
             return false;
         }
+
+        // latch the field strength check to pass if passed for 3 continuous seconds
+        // this allows the copter to be picked up from the ground to clear the check
+        field_pass_latched = (last_field_pass_time_ms != 0 && (millis() - last_field_fail_time_ms) > 3000) || field_pass_latched;
+        if (!field_pass_latched) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
+            }
+            return false;
+        }
     }
 
     // check GPS