AP_Compass: add method to kill the magnetometer inflight

bugobliterator · Jul 3, 2024 · 8da51b5 · 8da51b5
1 parent 009bb92
commit 8da51b5
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 0 deletions.
diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
@@ -2228,6 +2228,20 @@ void Compass::force_save_calibration(void)
     }
 }
 
+#if AP_COMPASS_KILL_MAG_ENABLED
+/*
+  update IMU kill mask, used for testing IMU failover
+ */
+void Compass::kill_mag(uint8_t mag_idx, bool kill_it)
+{
+    if (kill_it) {
+        mag_kill_mask |= (1U<<mag_idx);
+    } else {
+        mag_kill_mask &= ~(1U<<mag_idx);
+    }
+}
+#endif // AP_COMPASS_KILL_MAG_ENABLED
+
 // singleton instance
 Compass *Compass::_singleton;
 

diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
@@ -365,6 +365,10 @@ friend class AP_Compass_Backend;
     // get the first compass marked for use by COMPASSx_USE
     uint8_t get_first_usable(void) const { return _first_usable; }
 
+
+    // for killing an Mag for testing purposes
+    void kill_mag(uint8_t mag_idx, bool kill_it);
+
 private:
     static Compass *_singleton;
 
@@ -663,6 +667,8 @@ friend class AP_Compass_Backend;
     bool suppress_devid_save;
 
     uint8_t _first_usable; // first compass usable based on COMPASSx_USE param
+
+    uint8_t mag_kill_mask; // bitmask of mags to kill
 };
 
 namespace AP {

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -60,6 +60,10 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
 
 void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance)
 {
+    if ((1U<<instance) & _compass.mag_kill_mask) {
+        return;
+    }
+
     // note that we do not set last_update_usec here as otherwise the
     // EKF and DCM would end up consuming compass data at the full
     // sensor rate. We want them to consume only the filtered fields
@@ -139,6 +143,10 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
 void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
                                            uint32_t max_samples)
 {
+    if ((1U<<instance) & _compass.mag_kill_mask) {
+        return;
+    }
+
     /* rotate raw_field from sensor frame to body frame */
     rotate_field(field, instance);
 
@@ -166,6 +174,10 @@ void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint8_t instance,
 void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
                                                    const Vector3f *scaling)
 {
+    if ((1U<<instance) & _compass.mag_kill_mask) {
+        return;
+    }
+
     WITH_SEMAPHORE(_sem);
 
     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
@@ -190,6 +202,10 @@ void AP_Compass_Backend::drain_accumulated_samples(uint8_t instance,
  */
 void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance)
 {
+    if ((1U<<instance) & _compass.mag_kill_mask) {
+        return;
+    }
+
     Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
 
     state.field = mag;

diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h
@@ -127,3 +127,7 @@
 #ifndef AP_COMPASS_RM3100_ENABLED
 #define AP_COMPASS_RM3100_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_COMPASS_KILL_MAG_ENABLED
+#define AP_COMPASS_KILL_MAG_ENABLED AP_COMPASS_ENABLED
+#endif