Copter: set pre_arm_gps_check flag

ArduCopter/motors.pde

@@ -203,6 +203,7 @@ static void pre_arm_checks(bool display_failure)
 {
     // exit immediately if we've already successfully performed the pre-arm check
     if (ap.pre_arm_check) {
+        pre_arm_gps_checks(display_failure);
         return;
     }
 
@@ -299,18 +300,16 @@ static void pre_arm_checks(bool display_failure)
     }
 
     // check GPS
-    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
+    if (!pre_arm_gps_checks(display_failure)) {
-        // check gps is ok if required - note this same check is repeated again in arm_checks
+        return;
-        if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
+    }
-            return;
-        }
 
-#if AC_FENCE == ENABLED
+    // check fence is initialised
-        // check fence is initialised
+    if(!fence.pre_arm_check()) {
-        if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
+        if (display_failure) {
-            return;
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check fence"));
         }
-#endif
+        return;
     }
 
     // check INS
@@ -466,13 +465,39 @@ static void pre_arm_rc_checks()
 // performs pre_arm gps related checks and returns true if passed
 static bool pre_arm_gps_checks(bool display_failure)
 {
-    float speed_cms = inertial_nav.get_velocity().length();     // speed according to inertial nav in cm/s
+    // return true immediately if gps check is disabled
+    if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
+    // check if flight mode requires GPS
+    bool gps_required = mode_requires_GPS(control_mode);
+
+    // if GPS failsafe will triggers even in stabilize mode we need GPS before arming
+    if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
+        gps_required = true;
+    }
+
+#if AC_FENCE == ENABLED
+    // if circular fence is enabled we need GPS
+    if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
+        gps_required = true;
+    }
+#endif
+
+    // return true if GPS is not required
+    if (!gps_required) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
 
     // check GPS is not glitching
     if (gps_glitch.glitching()) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -481,14 +506,17 @@ static bool pre_arm_gps_checks(bool display_failure)
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
     // check speed is below 50cm/s
+    float speed_cms = inertial_nav.get_velocity().length();     // speed according to inertial nav in cm/s
     if (speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad Velocity"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -497,10 +525,12 @@ static bool pre_arm_gps_checks(bool display_failure)
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
         }
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
     // if we got here all must be ok
+    AP_Notify::flags.pre_arm_gps_check = true;
     return true;
 }
 
@@ -558,11 +588,9 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs)
         }
     }
 
-    // check gps is ok if required - note this same check is also done in pre-arm checks
+    // check gps
-    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
+    if (!pre_arm_gps_checks(display_failure)) {
-        if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
+        return false;
-            return false;
-        }
     }
 
     // check parameters