From cb39bd72d357253752a46ce7292d1045eeddd1e4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 13 Dec 2022 17:47:04 +0900 Subject: [PATCH] AP_Arming: only compare AHRS vs GPS if GPS is enabled --- libraries/AP_Arming/AP_Arming.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 92f8e2ba85..69f4d45f9c 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -598,13 +598,15 @@ bool AP_Arming::gps_checks(bool report) } // check AHRS and GPS are within 10m of each other - const Location gps_loc = gps.location(); - Location ahrs_loc; - if (AP::ahrs().get_location(ahrs_loc)) { - const float distance = gps_loc.get_distance(ahrs_loc); - if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) { - check_failed(ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance); - return false; + if (gps.num_sensors() > 0) { + const Location gps_loc = gps.location(); + Location ahrs_loc; + if (AP::ahrs().get_location(ahrs_loc)) { + const float distance = gps_loc.get_distance(ahrs_loc); + if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) { + check_failed(ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance); + return false; + } } } }