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
3drobotics · Sep 20, 2016 · 13d4329 · 13d4329
1 parent 696a95c
commit 13d4329
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 10 deletions.
diff --git 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
@@ -301,30 +301,41 @@ 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) {
                 gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent compasses"));
             }
             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