From 98efcd5f03b71dc009e9e1b038cce209c9b333a1 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 19 Mar 2015 14:58:13 +0900
Subject: [PATCH] AHRS: always use EKF for copter

---
 libraries/AP_AHRS/AP_AHRS.cpp        |  6 +++++-
 libraries/AP_AHRS/AP_AHRS.h          | 11 +++++++++--
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp |  7 +++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  2 +-
 4 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 092db6c60d..8fcc519f0c 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -18,6 +18,10 @@
 #include <AP_HAL.h>
 extern const AP_HAL::HAL& hal;
 
+#if AHRS_EKF_USE_ALWAYS
+const int8_t AP_AHRS::_ekf_use;
+#endif
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 	// index 0 and 1 are for old parameters that are no longer not used
@@ -111,7 +115,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
     // of 1 was found to be the best choice
 
-#if AP_AHRS_NAVEKF_AVAILABLE
+#if AP_AHRS_NAVEKF_AVAILABLE && !AHRS_EKF_USE_ALWAYS
     // @Param: EKF_USE
     // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
     // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index f21921cf19..5b17107cee 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -35,11 +35,13 @@
 
 // Copter defaults to EKF on by default, all others off
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
-#define AHRS_EKF_USE_DEFAULT    1
+ # define AHRS_EKF_USE_ALWAYS     1
 #else
-#define AHRS_EKF_USE_DEFAULT    0
+ # define AHRS_EKF_USE_ALWAYS     0
 #endif
 
+#define AHRS_EKF_USE_DEFAULT    0
+
 #define AP_AHRS_TRIM_LIMIT 10.0f        // maximum trim angle in degrees
 #define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
 #define AP_AHRS_YAW_P_MIN  0.05f        // minimum value for AHRS_YAW_P parameter
@@ -368,7 +370,12 @@ protected:
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
     AP_Int8 _gps_delay;
+
+#if AHRS_EKF_USE_ALWAYS
+    static const int8_t _ekf_use = 1;
+#else
     AP_Int8 _ekf_use;
+#endif
 
     // flags structure
     struct ahrs_flags {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 13ce921cef..c3648532be 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -312,6 +312,13 @@ bool AP_AHRS_NavEKF::healthy(void) const
     return AP_AHRS_DCM::healthy();    
 }
 
+void AP_AHRS_NavEKF::set_ekf_use(bool setting)
+{
+#if !AHRS_EKF_USE_ALWAYS
+    _ekf_use.set(setting);
+#endif
+}
+
 // true if the AHRS has completed initialisation
 bool AP_AHRS_NavEKF::initialised(void) const
 {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index d41f6b1441..943db6aa7a 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -111,7 +111,7 @@ public:
     // get speed limit
     void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler);
 
-    void set_ekf_use(bool setting) { _ekf_use.set(setting); }
+    void set_ekf_use(bool setting);
 
     // is the AHRS subsystem healthy?
     bool healthy(void) const;