From 4fdbbd89840cef2f54eb52a73e34a6927d10ce86 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 27 Nov 2020 09:37:13 +1100
Subject: [PATCH] AP_NavEKF: added functions to see if a nav source is enabled

this allows us to avoid initialising ring buffers when not needed
---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 47 ++++++++++++++++++++++++
 libraries/AP_NavEKF/AP_NavEKF_Source.h   | 11 +++++-
 2 files changed, 57 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index d29d370aef..1d5294b163 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -477,3 +477,50 @@ bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
 
     return true;
 }
+
+// return true if ext nav is enabled on any source
+bool AP_NavEKF_Source::ext_nav_enabled(void) const
+{
+    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+        const auto &src = _source_set[i];
+        if (SourceXY(src.posxy.get()) == SourceXY::EXTNAV) {
+            return true;
+        }
+        if (SourceZ(src.posz.get()) == SourceZ::EXTNAV) {
+            return true;
+        }
+        if (SourceXY(src.velxy.get()) == SourceXY::EXTNAV) {
+            return true;
+        }
+        if (SourceZ(src.velz.get()) == SourceZ::EXTNAV) {
+            return true;
+        }
+    }
+    return false;
+}
+
+// return true if wheel encoder is enabled on any source
+bool AP_NavEKF_Source::wheel_encoder_enabled(void) const
+{
+    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+        const auto &src = _source_set[i];
+        if (SourceXY(src.velxy.get()) == SourceXY::WHEEL_ENCODER) {
+            return true;
+        }
+    }
+    return false;
+}
+
+// return true if ext yaw is enabled on any source
+bool AP_NavEKF_Source::ext_yaw_enabled(void) const
+{
+    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+        const auto &src = _source_set[i];
+        const SourceYaw yaw = SourceYaw(src.yaw.get());
+        if (yaw == SourceYaw::EXTERNAL ||
+            yaw == SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h
index 925b772262..cd35a74e5a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.h
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h
@@ -93,10 +93,19 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    // return true if ext nav is enabled on any source
+    bool ext_nav_enabled(void) const;
+
+    // return true if ext yaw is enabled on any source
+    bool ext_yaw_enabled(void) const;
+    
+    // return true if wheel encoder is enabled on any source
+    bool wheel_encoder_enabled(void) const;
+    
 private:
 
     // Parameters
-    struct {
+    struct SourceSet {
         AP_Int8 posxy;  // xy position source
         AP_Int8 velxy;  // xy velocity source
         AP_Int8 posz;   // position z (aka altitude or height) source