From bde1b6e8862ee2fc5704032860a5d48e4eca4865 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 29 Aug 2017 13:33:33 -0700 Subject: [PATCH] AP_Arming: Collapse GPS checks into the same branch --- libraries/AP_Arming/AP_Arming.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index ba32b8fcf6..8c3a1e0923 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -357,23 +357,8 @@ bool AP_Arming::gps_checks(bool report) } return false; } - } - if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS_CONFIG)) { - uint8_t first_unconfigured = gps.first_unconfigured_gps(); - if (first_unconfigured != AP_GPS::GPS_ALL_CONFIGURED) { - if (report) { - gcs().send_text(MAV_SEVERITY_CRITICAL, - "PreArm: GPS %d failing configuration checks", - first_unconfigured + 1); - gps.broadcast_first_configuration_failure_reason(); - } - return false; - } - } - - // check GPSs are within 50m of each other and that blending is healthy - if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS)) { + // check GPSs are within 50m of each other and that blending is healthy float distance_m; if (!gps.all_consistent(distance_m)) { if (report) { @@ -391,6 +376,19 @@ bool AP_Arming::gps_checks(bool report) } } + if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS_CONFIG)) { + uint8_t first_unconfigured = gps.first_unconfigured_gps(); + if (first_unconfigured != AP_GPS::GPS_ALL_CONFIGURED) { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL, + "PreArm: GPS %d failing configuration checks", + first_unconfigured + 1); + gps.broadcast_first_configuration_failure_reason(); + } + return false; + } + } + return true; }