From 3f2a72f9db8852da283ba760b7b2a235eb1ddf16 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Jan 2022 07:55:36 +1100
Subject: [PATCH] AP_Compass: only magcal detected compasses

inflight compass learning with COMPASS_LEARN=3 was not working with
less than 3 compasses, as the force_use parameter was preventing
skipping compasses. This fixes it to calibate all detected compasses
that have USE enabled
---
 libraries/AP_Compass/AP_Compass_Calibration.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 8b727bd116..925246bd6e 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -521,12 +521,12 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
     // Rotate into body frame using provided yaw
     field = dcm.transposed() * field;
 
-    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+    for (uint8_t i=0; i<get_count(); i++) {
         if (compass_mask != 0 && ((1U<<i) & compass_mask) == 0) {
             // skip this compass
             continue;
         }
-        if (!force_use && !use_for_yaw(i)) {
+        if (_use_for_yaw[Priority(i)] == 0 || (!force_use && !use_for_yaw(i))) {
             continue;
         }
         if (!healthy(i)) {