From 878e84a01519e6f46bdeda32406933ac87b056e3 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 19 Mar 2018 16:58:40 -0700
Subject: [PATCH] AP_Compass: correct edge case where checks pass when saved
 dev_id != detected dev_id

---
 libraries/AP_Compass/AP_Compass.cpp         | 25 ++++++++++++++++-----
 libraries/AP_Compass/AP_Compass.h           |  1 +
 libraries/AP_Compass/AP_Compass_Backend.cpp |  1 +
 3 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 0135348fa4..8e636ed2b0 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -471,6 +471,13 @@ Compass::Compass(void)
     }
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
+
+    // Reset dev_id parameters' cached values to zero. This is here to address a potential
+    // edge case where a previously-detected compass is no longer present, but its dev_id
+    // is loaded from the parameter's stored value.
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        _state[i].dev_id = 0;
+    }
 }
 
 // Default init method
@@ -1084,6 +1091,7 @@ void
 Compass::save_offsets(uint8_t i)
 {
     _state[i].offset.save();  // save offsets
+    _state[i].dev_id.set_and_save(_state[i].detected_dev_id);
     _state[i].dev_id.save();  // save device id corresponding to these offsets
 }
 
@@ -1207,16 +1215,21 @@ bool Compass::configured(uint8_t i)
         return false;
     }
 
-    // backup detected dev_id
-    int32_t dev_id_orig = _state[i].dev_id;
+    // exit immediately if dev_id hasn't been detected
+    if (_state[i].detected_dev_id == 0) {
+        return false;
+    }
+
+    // back up cached value of dev_id
+    int32_t dev_id_cache_value = _state[i].dev_id;
 
     // load dev_id from eeprom
     _state[i].dev_id.load();
 
-    // if different then the device has not been configured
-    if (_state[i].dev_id != dev_id_orig) {
-        // restore device id
-        _state[i].dev_id = dev_id_orig;
+    // if dev_id loaded from eeprom is different from detected dev id or dev_id loaded from eeprom is different from cached dev_id, compass is unconfigured
+    if (_state[i].dev_id != _state[i].detected_dev_id || _state[i].dev_id != dev_id_cache_value) {
+        // restore cached value
+        _state[i].dev_id = dev_id_cache_value;
         // return failure
         return false;
     }
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 29180af86a..e8944a6f4f 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -431,6 +431,7 @@ private:
         // saved to eeprom when offsets are saved allowing ram &
         // eeprom values to be compared as consistency check
         AP_Int32    dev_id;
+        int32_t detected_dev_id;
 
         AP_Int8     use_for_yaw;
 
diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index b0875d3112..83eb1e46f2 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -130,6 +130,7 @@ uint8_t AP_Compass_Backend::register_compass(void) const
 void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
 {
     _compass._state[instance].dev_id.set_and_notify(dev_id);
+    _compass._state[instance].detected_dev_id = dev_id;
 }
 
 /*