AP_Compass: added get_num_enabled()

used by EKF3

libraries/AP_Compass/AP_Compass.cpp

@@ -1478,6 +1478,24 @@ Compass::use_for_yaw(uint8_t i) const
     return _use_for_yaw[Priority(i)] && _learn.get() != LEARN_INFLIGHT;
 }
 
+/*
+  return the number of enabled sensors. Used to determine if
+  non-compass operation is desired
+ */
+uint8_t Compass::get_num_enabled(void) const
+{
+    if (get_count() == 0) {
+        return 0;
+    }
+    uint8_t count = 0;
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if (use_for_yaw(i)) {
+            count++;
+        }
+    }
+    return count;
+}
+
 void
 Compass::set_use_for_yaw(uint8_t i, bool use)
 {

libraries/AP_Compass/AP_Compass.h

@@ -137,6 +137,9 @@ public:
     // return the number of compass instances
     uint8_t get_count(void) const { return _compass_count; }
 
+    // return the number of enabled sensors
+    uint8_t get_num_enabled(void) const;
+    
     /// Return the current field as a Vector3f in milligauss
     const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; }
     const Vector3f &get_field(void) const { return get_field(0); }