From d343c569c28e31c0b3d01ebbe3e1e0d2963ccd3c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Mar 2021 17:22:03 +0900 Subject: [PATCH] AP_Arming: check for only first compass being disabled If only the first compass is disabled, users may expect other enabled compasses to be used but they won't be --- libraries/AP_Arming/AP_Arming.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index e0aeb5b595..989c4c1539 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -414,6 +414,12 @@ bool AP_Arming::compass_checks(bool report) if ((checks_to_perform) & ARMING_CHECK_ALL || (checks_to_perform) & ARMING_CHECK_COMPASS) { + // check for first compass being disabled but 2nd or 3rd being enabled + if (!_compass.use_for_yaw(0) && (_compass.get_num_enabled() > 0)) { + check_failed(ARMING_CHECK_COMPASS, report, "Compass1 disabled but others enabled"); + return false; + } + // avoid Compass::use_for_yaw(void) as it implicitly calls healthy() which can // incorrectly skip the remaining checks, pass the primary instance directly if (!_compass.use_for_yaw(0)) {