From 9fc9238c5321eb58ee5045910b19c673cd79bb10 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Sep 2023 09:03:24 +1000
Subject: [PATCH] AP_Compass: prevent saving of device IDs when not calibrated

this fixes an issue with the following sequence:

 - new board (or board with FORMAT_VERSION reset) starts up with only internal compasses
 - internal compasses are detected and devids saved
 - an external compass is added and the board is rebooted
 - the external compass will not be the first compass
 - user then calibrates and flies, but has internal as primary

this can lead to a very bad experience for new users. At least one
vehicle has crashed due to this sequence.

The fix is to not save device IDs during the Compass::init() if we
have never been calibrated. This means that when an external compass
is added it will come up as the first compass.

This also removes the saving of the extra device ID. It was never
intended that these be saved (there is a comment to that effect in the
code), but actually they were saved.
---
 libraries/AP_Compass/AP_Compass.cpp | 81 +++++++++++++++++------------
 libraries/AP_Compass/AP_Compass.h   |  2 +
 2 files changed, 51 insertions(+), 32 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 8a9539a4c3..ba1ca2a602 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -697,6 +697,25 @@ void Compass::init()
         return;
     }
 
+    /*
+      on init() if any devid is set then we set suppress_devid_save to
+      false. This is used to determine if we save device ids during
+      the init process.
+     */
+    suppress_devid_save = true;
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if (_state._priv_instance[i].dev_id != 0) {
+            suppress_devid_save = false;
+            break;
+        }
+#if COMPASS_MAX_INSTANCES > 1
+        if (_priority_did_stored_list._priv_instance[i] != 0) {
+            suppress_devid_save = false;
+            break;
+        }
+#endif
+    }
+
     // convert to new custom rotation method
     // PARAMETER_CONVERSION - Added: Nov-2021
 #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
@@ -741,24 +760,26 @@ void Compass::init()
 
     // Load priority list from storage, the changes to priority list
     // by user only take effect post reboot, after this
-    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        if (_priority_did_stored_list[i] != 0) {
-            _priority_did_list[i] = _priority_did_stored_list[i];
-        } else {
-            // Maintain a list without gaps and duplicates
-            for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++) {
-                int32_t temp;
-                if (_priority_did_stored_list[j] == _priority_did_stored_list[i]) {
+    if (!suppress_devid_save) {
+        for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
+            if (_priority_did_stored_list[i] != 0) {
+                _priority_did_list[i] = _priority_did_stored_list[i];
+            } else {
+                // Maintain a list without gaps and duplicates
+                for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++) {
+                    int32_t temp;
+                    if (_priority_did_stored_list[j] == _priority_did_stored_list[i]) {
+                        _priority_did_stored_list[j].set_and_save_ifchanged(0);
+                    }
+                    if (_priority_did_stored_list[j] == 0) {
+                        continue;
+                    }
+                    temp = _priority_did_stored_list[j];
                     _priority_did_stored_list[j].set_and_save_ifchanged(0);
+                    _priority_did_list[i] = temp;
+                    _priority_did_stored_list[i].set_and_save_ifchanged(temp);
+                    break;
                 }
-                if (_priority_did_stored_list[j] == 0) {
-                    continue;
-                }
-                temp = _priority_did_stored_list[j];
-                _priority_did_stored_list[j].set_and_save_ifchanged(0);
-                _priority_did_list[i] = temp;
-                _priority_did_stored_list[i].set_and_save_ifchanged(temp);
-                break;
             }
         }
     }
@@ -777,7 +798,7 @@ void Compass::init()
         // cache the extra devices detected in last boot
         // for detecting replacement mag
         _previously_unreg_mag[i] = extra_dev_id[i];
-        extra_dev_id[i].set_and_save(0);
+        extra_dev_id[i].set(0);
     }
 #endif
 
@@ -786,7 +807,9 @@ void Compass::init()
     // which are set() but not saved() during normal runtime,
     // do not move this call without ensuring that is not happening
     // read comments under set_and_save_ifchanged for details
-    _reorder_compass_params();
+    if (!suppress_devid_save) {
+        _reorder_compass_params();
+    }
 #endif
 
     if (_compass_count == 0) {
@@ -794,16 +817,6 @@ void Compass::init()
         _detect_backends();
     }
 
-#if COMPASS_MAX_UNREG_DEV
-    // We store the list of unregistered mags detected here,
-    // We don't do this during runtime, as we don't want to detect
-    // compasses connected by user as a replacement while the system
-    // is running
-    for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
-        extra_dev_id[i].save();
-    }
-#endif
-
     if (_compass_count != 0) {
         // get initial health status
         hal.scheduler->delay(100);
@@ -826,6 +839,7 @@ void Compass::init()
 #endif
 
     init_done = true;
+    suppress_devid_save = false;
 }
 
 #if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
@@ -846,7 +860,11 @@ Compass::Priority Compass::_update_priority_list(int32_t dev_id)
     // We are not in priority list, let's add at first empty
     for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
         if (_priority_did_stored_list[i] == 0) {
-            _priority_did_stored_list[i].set_and_save(dev_id);
+            if (suppress_devid_save) {
+                _priority_did_stored_list[i].set(dev_id);
+            } else {
+                _priority_did_stored_list[i].set_and_save(dev_id);
+            }
             _priority_did_list[i] = dev_id;
             if (i >= _compass_count) {
                 _compass_count = uint8_t(i)+1;
@@ -1463,7 +1481,7 @@ void Compass::_detect_backends(void)
         }
 
 #if COMPASS_MAX_UNREG_DEV > 0
-        if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT)) {
+        if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT) && !suppress_devid_save) {
             // 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++) {
@@ -1570,7 +1588,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid)
 #if COMPASS_MAX_UNREG_DEV > 0
     for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
         if ((uint32_t)extra_dev_id[i] == devid) {
-            extra_dev_id[i].set_and_save(0);
+            extra_dev_id[i].set(0);
             return;
         }
     }
@@ -2153,7 +2171,6 @@ void Compass::force_save_calibration(void)
     }
 }
 
-
 // singleton instance
 Compass *Compass::_singleton;
 
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index fd37550b41..f6dbe68e2c 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -649,6 +649,8 @@ private:
 #endif
     bool init_done;
 
+    bool suppress_devid_save;
+
     uint8_t _first_usable; // first compass usable based on COMPASSx_USE param
 };