From f31cf7debe8b2071803801fe264935fd169d6c27 Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Tue, 21 Jul 2020 02:55:08 +0530
Subject: [PATCH] AP_Compass: reset compass ids not present after compass cal

also implement replacement mechanism for UAVCAN compasses
---
 libraries/AP_Compass/AP_Compass.cpp           | 158 +++++++++++++++++-
 libraries/AP_Compass/AP_Compass.h             |  11 ++
 libraries/AP_Compass/AP_Compass_Backend.cpp   |   1 -
 .../AP_Compass/AP_Compass_Calibration.cpp     |   2 +
 libraries/AP_Compass/AP_Compass_UAVCAN.cpp    |  17 +-
 libraries/AP_Compass/AP_Compass_UAVCAN.h      |   6 +-
 6 files changed, 180 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index c213c8b8c5..08060e984b 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -696,9 +696,12 @@ void Compass::init()
         if (_priority_did_stored_list[i] != 0) {
             _priority_did_list[i] = _priority_did_stored_list[i];
         } else {
-            // Maintain a list without gaps
+            // 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(0);
+                }
                 if (_priority_did_stored_list[j] == 0) {
                     continue;
                 }
@@ -722,7 +725,10 @@ void Compass::init()
     // interface for users to see unreg compasses, we actually never store it
     // in storage.
     for (uint8_t i=_unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) {
-        extra_dev_id[i].set(0);
+        // 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);
     }
 #endif
 
@@ -733,6 +739,16 @@ 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);
@@ -813,6 +829,7 @@ void Compass::mag_state::copy_from(const Compass::mag_state& state)
     dev_id.set_and_save(state.dev_id);
     motor_compensation.set_and_save(state.motor_compensation);
     expected_dev_id = state.expected_dev_id;
+    detected_dev_id = state.detected_dev_id;
 }
 //  Register a new compass instance
 //
@@ -900,7 +917,7 @@ Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const
         return StateIndex(COMPASS_MAX_INSTANCES);
     }
     for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
-        if (_priority_did_list[priority] == _state[i].expected_dev_id) {
+        if (_priority_did_list[priority] == _state[i].detected_dev_id) {
             return i;
         }
     }
@@ -1251,8 +1268,68 @@ void Compass::_detect_backends(void)
             if (_uavcan_backend) {
                 _add_backend(_uavcan_backend);
             }
-            CHECK_UNREG_LIMIT_RETURN;
+#if COMPASS_MAX_UNREG_DEV > 0
+            if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV)  {
+                break;
+            }
+#endif
         }
+
+#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_UAVCAN::get_detected_devid(j);
+                // Check if this is a potential replacement mag
+                if (!is_replacement_mag(detected_devid)) {
+                    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_UAVCAN::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;
+                }
+            }
+        }
+#endif
     }
 #endif
 
@@ -1262,6 +1339,79 @@ void Compass::_detect_backends(void)
     }
 }
 
+// Check if the devid is a potential replacement compass
+// Following are the checks done to ensure the compass is a replacement
+// * The compass is an UAVCAN compass
+// * The compass wasn't seen before this boot as additional unreg mag
+// * The compass might have been seen before but never setup
+bool Compass::is_replacement_mag(uint32_t devid) {
+#if COMPASS_MAX_INSTANCES > 1
+    // We only do this for UAVCAN mag
+    if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
+        return false;
+    }
+
+    // Check that its not an unused additional mag
+    for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
+        if (_previously_unreg_mag[i] == devid) {
+            return false;
+        }
+    }
+
+    // Check that its not previously setup mag
+    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
+        if ((uint32_t)_state[i].expected_dev_id == devid) {
+            return false;
+        }
+    }
+#endif
+    return true;
+}
+
+void Compass::remove_unreg_dev_id(uint32_t devid)
+{
+#if COMPASS_MAX_INSTANCES > 1
+    // We only do this for UAVCAN mag
+    if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
+        return;
+    }
+
+    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);
+            return;
+        }
+    }
+#endif
+}
+
+void Compass::_reset_compass_id()
+{
+#if COMPASS_MAX_INSTANCES > 1
+    // Check if any of the registered devs are not registered
+    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
+        if (_priority_did_stored_list[i] != _priority_did_list[i] ||
+            _priority_did_stored_list[i] == 0) {
+            //We don't touch priorities that might have been touched by the user
+            continue;
+        }
+        if (!_get_state(i).registered) {
+            _priority_did_stored_list[i].set_and_save(0);
+            gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", uint8_t(i), (unsigned long)_priority_did_list[i]);
+        }
+    }
+
+    // Check if any of the old registered devs are not registered
+    // and hence can be removed
+    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
+        if (_state[i].dev_id == 0 && _state[i].expected_dev_id != 0) {
+            // also hard reset dev_ids that are not detected
+            _state[i].dev_id.save();
+        }
+    }
+#endif
+}
+
 // Look for devices beyond initialisation
 void
 Compass::_detect_runtime(void)
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 31b56cb3fa..b551defb07 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -481,7 +481,9 @@ private:
         // saved to eeprom when offsets are saved allowing ram &
         // eeprom values to be compared as consistency check
         AP_Int32    dev_id;
+        // Initialised when compass is detected
         int32_t detected_dev_id;
+        // Initialised at boot from saved devid
         int32_t expected_dev_id;
 
         // factors multiplied by throttle and added to compass outputs
@@ -525,6 +527,14 @@ private:
     // load them as they come up the first time
     Priority _update_priority_list(int32_t dev_id);
     
+    // method to check if the mag with the devid 
+    // is a replacement mag
+    bool is_replacement_mag(uint32_t dev_id);
+
+    //remove the devid from unreg compass list
+    void remove_unreg_dev_id(uint32_t devid);
+
+    void _reset_compass_id();
     //Create Arrays to be accessible by Priority only
     RestrictIDTypeArray<AP_Int8, COMPASS_MAX_INSTANCES, Priority> _use_for_yaw;
 #if COMPASS_MAX_INSTANCES > 1
@@ -560,6 +570,7 @@ private:
 #if COMPASS_MAX_UNREG_DEV
     // Put extra dev ids detected
     AP_Int32 extra_dev_id[COMPASS_MAX_UNREG_DEV];
+    uint32_t _previously_unreg_mag[COMPASS_MAX_UNREG_DEV];
 #endif
 
     AP_Int8 _filter_range;
diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index 48b55bb2ee..cba5958283 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -202,7 +202,6 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
 {
     _compass._state[Compass::StateIndex(instance)].dev_id.set_and_notify(dev_id);
     _compass._state[Compass::StateIndex(instance)].detected_dev_id = dev_id;
-    _compass._state[Compass::StateIndex(instance)].expected_dev_id = dev_id;
 }
 
 /*
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 442892c4b2..86b69e127b 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -329,6 +329,7 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
         bool autoreboot = !is_zero(packet.param5);
 
         if (mag_mask == 0) { // 0 means all
+            _reset_compass_id();
             start_calibration_all(retry, autosave, delay, autoreboot);
         } else {
             if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
@@ -429,6 +430,7 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field)
 MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
                                       float lat_deg, float lon_deg)
 {
+    _reset_compass_id();
     if (is_zero(lat_deg) && is_zero(lon_deg)) {
         Location loc;
         // get AHRS position. If unavailable then try GPS location
diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp
index 3e66c31edf..e06c41551b 100644
--- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp
+++ b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp
@@ -37,10 +37,11 @@ UC_REGISTRY_BINDER(Mag2Cb, uavcan::equipment::ahrs::MagneticFieldStrength2);
 AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[] = {0};
 HAL_Semaphore AP_Compass_UAVCAN::_sem_registry;
 
-AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id)
+AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id, uint32_t devid)
     : _ap_uavcan(ap_uavcan)
     , _node_id(node_id)
     , _sensor_id(sensor_id)
+    , _devid(devid)
 {
 }
 
@@ -75,7 +76,7 @@ AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index)
     if (!_detected_modules[index].driver && _detected_modules[index].ap_uavcan) {
         WITH_SEMAPHORE(_sem_registry);
         // Register new Compass mode to a backend
-        driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id);
+        driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id, _detected_modules[index].devid);
         if (driver) {
             if (!driver->init()) {
                 delete driver;
@@ -95,16 +96,12 @@ AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index)
 
 bool AP_Compass_UAVCAN::init()
 {
-    uint32_t devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
-                                                 _ap_uavcan->get_driver_index(),
-                                                 _node_id,
-                                                 _sensor_id + 1); // we use sensor_id as devtype
     // Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default
-    if (!register_compass(devid, _instance)) {
+    if (!register_compass(_devid, _instance)) {
         return false;
     }
 
-    set_dev_id(_instance, devid);
+    set_dev_id(_instance, _devid);
     set_external(_instance, true);
 
     debug_mag_uavcan(2, _ap_uavcan->get_driver_index(),  "AP_Compass_UAVCAN loaded\n\r");
@@ -142,6 +139,10 @@ AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, u
                 _detected_modules[i].ap_uavcan = ap_uavcan;
                 _detected_modules[i].node_id = node_id;
                 _detected_modules[i].sensor_id = sensor_id;
+                _detected_modules[i].devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
+                                                 ap_uavcan->get_driver_index(),
+                                                 node_id,
+                                                 sensor_id + 1); // we use sensor_id as devtype
                 break;
             }
         }
diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h
index f487b4287c..d469b03597 100644
--- a/libraries/AP_Compass/AP_Compass_UAVCAN.h
+++ b/libraries/AP_Compass/AP_Compass_UAVCAN.h
@@ -10,13 +10,13 @@ class Mag2Cb;
 
 class AP_Compass_UAVCAN : public AP_Compass_Backend {
 public:
-    AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id);
+    AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id, uint32_t devid);
 
     void        read(void) override;
 
     static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
     static AP_Compass_Backend* probe(uint8_t index);
-
+    static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
     static void handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb);
     static void handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb);
 
@@ -33,6 +33,7 @@ private:
     AP_UAVCAN* _ap_uavcan;
     uint8_t _node_id;
     uint8_t _sensor_id;
+    uint32_t _devid;
 
     // Module Detection Registry
     static struct DetectedModules {
@@ -40,6 +41,7 @@ private:
         uint8_t node_id;
         uint8_t sensor_id;
         AP_Compass_UAVCAN *driver;
+        uint32_t devid;
     } _detected_modules[COMPASS_MAX_BACKEND];
 
     static HAL_Semaphore _sem_registry;