From a93b93b741adc35b63ce5e78d62b3099b70e90e7 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Fri, 5 Jun 2020 15:17:54 +0530
Subject: [PATCH] AP_Compass: return id of max empty compass state when prio id
 is 0

---
 libraries/AP_Compass/AP_Compass.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index e5acdb8fb8..c6c3e58877 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -896,6 +896,9 @@ bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
 Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const
 {
 #if COMPASS_MAX_INSTANCES > 1
+    if (_priority_did_list[priority] == 0) {
+        return StateIndex(COMPASS_MAX_INSTANCES);
+    }
     for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
         if (_priority_did_list[priority] == _state[i].expected_dev_id) {
             return i;
@@ -1569,7 +1572,8 @@ bool Compass::configured(uint8_t i)
 
     StateIndex id = _get_state_id(Priority(i));
     // exit immediately if dev_id hasn't been detected
-    if (_state[id].detected_dev_id == 0) {
+    if (_state[id].detected_dev_id == 0 || 
+        id == COMPASS_MAX_INSTANCES) {
         return false;
     }