From da7d34224de3e7d0a29443e9b6f99357dec61eb1 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 3 Mar 2021 14:20:09 +0530 Subject: [PATCH] Copter: do common gps arming checks first before moving on --- ArduCopter/AP_Arming.cpp | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 91143e5d8f..24b1350885 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -417,15 +417,6 @@ bool AP_Arming_Copter::rc_calibration_checks(bool display_failure) // performs pre_arm gps related checks and returns true if passed bool AP_Arming_Copter::gps_checks(bool display_failure) { - // run mandatory gps checks first - if (!mandatory_gps_checks(display_failure)) { - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // check if flight mode requires GPS - bool mode_requires_gps = copter.flightmode->requires_GPS(); - // check if fence requires GPS bool fence_requires_gps = false; #if AC_FENCE == ENABLED @@ -433,6 +424,23 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; #endif + // check if flight mode requires GPS + bool mode_requires_gps = copter.flightmode->requires_GPS(); + + // call parent gps checks + if (mode_requires_gps || fence_requires_gps) { + if (!AP_Arming::gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + } + + // run mandatory gps checks first + if (!mandatory_gps_checks(display_failure)) { + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + // return true if GPS is not required if (!mode_requires_gps && !fence_requires_gps) { AP_Notify::flags.pre_arm_gps_check = true; @@ -452,12 +460,6 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) return false; } - // call parent gps checks - if (!AP_Arming::gps_checks(display_failure)) { - 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;