Browse Source

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
zr-v5.1
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
commit
d343c569c2
  1. 6
      libraries/AP_Arming/AP_Arming.cpp

6
libraries/AP_Arming/AP_Arming.cpp

@ -414,6 +414,12 @@ bool AP_Arming::compass_checks(bool report) @@ -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)) {

Loading…
Cancel
Save