Copter: pre-arm check for mag field length

ArduCopter/config.h

@@ -421,6 +421,17 @@
  # define MAGNETOMETER                   ENABLED
 #endif
 
+// expected magnetic field strength
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ #ifndef COMPASS_MAGFIELD_EXPECTED
+  # define COMPASS_MAGFIELD_EXPECTED     330    // maximum mag field length for pre-arm checks
+ #endif
+#else // APM1, PX4, SITL
+ #ifndef COMPASS_MAGFIELD_EXPECTED
+  #define COMPASS_MAGFIELD_EXPECTED      530    // maximum mag field length for pre-arm checks
+ #endif
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  OPTICAL_FLOW
 #if defined( __AVR_ATmega2560__ )       // determines if optical flow code is included

ArduCopter/motors.pde

@@ -246,6 +246,17 @@ static void pre_arm_checks(bool display_failure)
         return;
     }
 
+    // check for unreasonable mag field length
+    float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
+    if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.5 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.5) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
+        }
+        return;
+    }else{
+        cliSerial->printf_P(PSTR("\nMagField: %4.2f vs %4.2f ~ %4.2f"),mag_field, (float)COMPASS_MAGFIELD_EXPECTED*1.5, COMPASS_MAGFIELD_EXPECTED*0.5);
+    }
+
     // barometer health check
     if(!barometer.healthy) {
         if (display_failure) {

ArduCopter/setup.pde

@@ -660,7 +660,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
         // calculate and display interference compensation at full throttle as % of total mag field
         if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
             // interference is impact@fullthrottle / mag field * 100
-            interference_pct = motor_compensation.length() / 330.0f * 100.0f;
+            interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
         }else{
             // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
             interference_pct = motor_compensation.length() * (current_amps_max/throttle_pct_max) / 330.0f * 100.0f;