From 6b3e114cd68cabb936fac0ab7a84cd84eb8f212c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Mon, 19 Oct 2015 10:05:55 +1100
Subject: [PATCH] AP_NavEKF: Enable EKF1 to be disabled to reduce frame
 over-runs

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 13 +++++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  1 +
 2 files changed, 14 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index c7c6d40457..8dd575e679 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -394,6 +394,14 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
     // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:horiz pos error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
     // @User: Advanced
     AP_GROUPINFO("GPS_CHECK",    33, NavEKF, _gpsCheck, 31),
+
+    // @Param: ENABLE
+    // @DisplayName: Enable EKF1
+    // @Description: This enables EKF1 to be disabled when using alternative algorithms. When disabling it, the alternate EKF2 estimator must be enabled by setting EK2_ENABLED = 1 and flight control algorithms must be set to use the alternative estimator by setting AHRS_EKF_TYPE = 2.
+    // @Values: 0:Disabled, 1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 34, NavEKF, _enable, 1),
+
     AP_GROUPEND
 };
 
@@ -545,6 +553,11 @@ void NavEKF::ResetHeight(void)
 // it should NOT be used to re-initialise after a timeout as DCM will also be corrupted
 bool NavEKF::InitialiseFilterDynamic(void)
 {
+    // Don't start if the user has disabled
+    if (_enable == 0) {
+        return false;
+    }
+
     // this forces healthy() to be false so that when we ask for ahrs
     // attitude we get the DCM attitude regardless of the state of AHRS_EKF_USE
     statesInitialised = false;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index f9392fac16..63eb49a02f 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -498,6 +498,7 @@ private:
     void calcPosDownDerivative();
 
     // EKF Mavlink Tuneable Parameters
+    AP_Int8  _enable;               // zero to disable EKF1
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s
     AP_Float _gpsHorizPosNoise;     // GPS horizontal position measurement noise m