diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 5cee41931a..0c833d3a15 100644 --- a/ArduCopter/motors.pde +++ b/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)) { - // check gps is ok if required - note this same check is repeated again in arm_checks - if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) { - return; - } + if (!pre_arm_gps_checks(display_failure)) { + return; + } -#if AC_FENCE == ENABLED - // check fence is initialised - if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) { - return; + // check fence is initialised + if(!fence.pre_arm_check()) { + if (display_failure) { + 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 - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) { - if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) { - return false; - } + // check gps + if (!pre_arm_gps_checks(display_failure)) { + return false; } // check parameters