From a6d03bb8a1b570d166128fe441a18faaef7c0085 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 13 Nov 2025 20:05:39 +1100
Subject: [PATCH] AP_Compass: fix issue with compass registration for more than
 3 compasses

This fixes an issue where we would fail to register a dronecan compass
if its not already in the stored DEV_ID position that matches the detection order

This is fixed by ensuring that we try our best to register the compass in the stored DEV_ID position
that matches the priority order.

This in turn allows for over 3 dronecan compasses to appear and not cause incorrect
registration failures at boot.
---
 libraries/AP_Compass/AP_Compass.cpp          | 11 +++++++++++
 libraries/AP_Compass/AP_Compass.h            |  1 +
 libraries/AP_Compass/AP_Compass_DroneCAN.cpp |  1 +
 3 files changed, 13 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 432bc434c65..3d2176df1c7 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -991,6 +991,17 @@ bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
 
     // This might be a replacement compass module, find any unregistered compass
     // instance and replace that
+    priority = _update_priority_list(dev_id);
+    // try to match priority and state index if possible, this ensure that compass order
+    // to state order while detection is preserved, this ensures that if compasses in priority
+    // list show up out of order during detection, it does not replace the state.
+    StateIndex priority_index = StateIndex(uint8_t(priority));
+    if (!_state[priority_index].registered && priority < COMPASS_MAX_INSTANCES) {
+        _state[priority_index].registered = true;
+        _state[priority_index].priority = priority;
+        instance = uint8_t(priority_index);
+        return true;
+    }
     for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
         priority = _update_priority_list(dev_id);
         if (!_state[i].registered && priority < COMPASS_MAX_INSTANCES) {
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 5c755aaa30f..e929b00cce0 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -72,6 +72,7 @@ class CompassLearn;
 class Compass
 {
 friend class AP_Compass_Backend;
+friend class AP_Compass_DroneCAN;
 public:
     Compass();
 
diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
index bfee98f7e08..0e6dfde47d1 100644
--- a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
+++ b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
@@ -154,6 +154,7 @@ AP_Compass_DroneCAN* AP_Compass_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_d
             }
         }
     }
+
     return nullptr;
 }