From 262b11f0c1c452ea9dca2d399f82351fe2a5f2fe Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 18 Jun 2023 19:19:39 +0100
Subject: [PATCH] AP_Compass: stop auto replacing missing CAN by default, add
 option to re-enable

---
 libraries/AP_Compass/AP_Compass.cpp           | 95 ++++++++++---------
 libraries/AP_Compass/AP_Compass.h             |  2 +
 .../AP_Compass/AP_Compass_Calibration.cpp     |  2 +-
 3 files changed, 52 insertions(+), 47 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 3501ebd17c..010b62428b 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -588,6 +588,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @DisplayName: Compass options
     // @Description: This sets options to change the behaviour of the compass
     // @Bitmask: 0:CalRequireGPS
+    // @Bitmask: 1: Allow missing DroneCAN compasses to be automaticaly replaced (calibration still required)
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0),
 
@@ -1464,56 +1465,58 @@ void Compass::_detect_backends(void)
         }
 
 #if COMPASS_MAX_UNREG_DEV > 0
-        // check if there's any uavcan compass in prio slot that's not found
-        // and replace it if there's a replacement compass
-        for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-            if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
-                || _get_state(i).registered) {
-                continue;
-            }
-            // There's a UAVCAN compass missing
-            // Let's check if there's a replacement
-            for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
-                uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
-                // Check if this is a potential replacement mag
-                if (!is_replacement_mag(detected_devid)) {
+        if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT)) {
+            // check if there's any uavcan compass in prio slot that's not found
+            // and replace it if there's a replacement compass
+            for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
+                if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
+                    || _get_state(i).registered) {
                     continue;
                 }
-                // We have found a replacement mag, let's replace the existing one
-                // with this by setting the priority to zero and calling uavcan probe 
-                gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
-                _priority_did_stored_list[i].set_and_save(0);
-                _priority_did_list[i] = 0;
-
-                AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
-                if (_uavcan_backend) {
-                    _add_backend(_uavcan_backend);
-                    // we also need to remove the id from unreg list
-                    remove_unreg_dev_id(detected_devid);
-                } else {
-                    // the mag has already been allocated,
-                    // let's begin the replacement
-                    bool found_replacement = false;
-                    for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
-                        if ((uint32_t)_state[k].dev_id == detected_devid) {
-                            if (_state[k].priority <= uint8_t(i)) {
-                                // we are already on higher priority
-                                // nothing to do
-                                break;
-                            }
-                            found_replacement = true;
-                            // reset old priority of replacement mag
-                            _priority_did_stored_list[_state[k].priority].set_and_save(0);
-                            _priority_did_list[_state[k].priority] = 0;
-                            // update new priority
-                            _state[k].priority = i;
-                        }
-                    }
-                    if (!found_replacement) {
+                // There's a UAVCAN compass missing
+                // Let's check if there's a replacement
+                for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
+                    uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
+                    // Check if this is a potential replacement mag
+                    if (!is_replacement_mag(detected_devid)) {
                         continue;
                     }
-                    _priority_did_stored_list[i].set_and_save(detected_devid);
-                    _priority_did_list[i] = detected_devid;
+                    // We have found a replacement mag, let's replace the existing one
+                    // with this by setting the priority to zero and calling uavcan probe 
+                    gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
+                    _priority_did_stored_list[i].set_and_save(0);
+                    _priority_did_list[i] = 0;
+
+                    AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
+                    if (_uavcan_backend) {
+                        _add_backend(_uavcan_backend);
+                        // we also need to remove the id from unreg list
+                        remove_unreg_dev_id(detected_devid);
+                    } else {
+                        // the mag has already been allocated,
+                        // let's begin the replacement
+                        bool found_replacement = false;
+                        for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
+                            if ((uint32_t)_state[k].dev_id == detected_devid) {
+                                if (_state[k].priority <= uint8_t(i)) {
+                                    // we are already on higher priority
+                                    // nothing to do
+                                    break;
+                                }
+                                found_replacement = true;
+                                // reset old priority of replacement mag
+                                _priority_did_stored_list[_state[k].priority].set_and_save(0);
+                                _priority_did_list[_state[k].priority] = 0;
+                                // update new priority
+                                _state[k].priority = i;
+                            }
+                        }
+                        if (!found_replacement) {
+                            continue;
+                        }
+                        _priority_did_stored_list[i].set_and_save(detected_devid);
+                        _priority_did_list[i] = detected_devid;
+                    }
                 }
             }
         }
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 609ee970b5..9ccbea0582 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -607,7 +607,9 @@ private:
     // bitmask of options
     enum class Option : uint16_t {
         CAL_REQUIRE_GPS = (1U<<0),
+        ALLOW_DRONECAN_AUTO_REPLACEMENT = (1U<<1),
     };
+    bool option_set(Option opt) const { return (_options.get() & uint16_t(opt)) != 0; }
     AP_Int16 _options;
 
 #if COMPASS_CAL_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 9943fc8740..b226e505fa 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -70,7 +70,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
         }
     }
 
-    if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
+    if (option_set(Option::CAL_REQUIRE_GPS)) {
         if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
             gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
             return false;