From 8c5cde4efd0f56e1c3d50920f28a24711ba9cd67 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 Jan 2014 13:05:28 +1100
Subject: [PATCH] AP_AHRS: allow NavEKF to be enabled at runtime with
 AHRS_EKF_USE=1

---
 libraries/AP_AHRS/AP_AHRS.cpp        |  9 +++++
 libraries/AP_AHRS/AP_AHRS.h          |  1 +
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 54 ++++++++++++++++++++++++++--
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  8 ++++-
 4 files changed, 68 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 96feccdc3a..8780e27e19 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -116,6 +116,15 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("GPS_DELAY", 12, AP_AHRS, _gps_delay, 1),
 
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // @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
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("EKF_USE",  13, AP_AHRS, _ekf_use, 0),
+#endif
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 5a734b63d3..b5127d444b 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -272,6 +272,7 @@ protected:
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
     AP_Int8 _gps_delay;
+    AP_Int8 _ekf_use;
 
     // flags structure
     struct ahrs_flags {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 3a4bf70bad..c513054f0b 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -23,6 +23,8 @@
 
 #if AP_AHRS_NAVEKF_AVAILABLE
 
+extern const AP_HAL::HAL& hal;
+
 // return the smoothed gyro vector corrected for drift
 const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
 {
@@ -31,7 +33,10 @@ const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
 
 const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
 {
-    return AP_AHRS_DCM::get_dcm_matrix();
+    if (!ekf_started) {
+        return AP_AHRS_DCM::get_dcm_matrix();
+    }
+    return _dcm_matrix;
 }
 
 const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
@@ -42,26 +47,64 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
 void AP_AHRS_NavEKF::update(void)
 {
     AP_AHRS_DCM::update();
+    if (!ekf_started) {
+        if (start_time_ms == 0) {
+            start_time_ms = hal.scheduler->millis();
+        }
+        // if we have GPS lock and a compass set we can start the EKF
+        if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D &&
+            hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
+            ekf_started = true;
+            EKF.InitialiseFilter();
+        }
+    }
+    if (ekf_started) {
+        EKF.UpdateFilter();
+        if (_ekf_use) {
+            EKF.getRotationBodyToNED(_dcm_matrix);
+            Vector3f eulers;
+            EKF.getEulerAngles(eulers);
+            roll  = eulers.x;
+            pitch = eulers.y;
+            yaw   = eulers.z;
+            roll_sensor  = degrees(roll) * 100;
+            pitch_sensor = degrees(pitch) * 100;
+            yaw_sensor   = degrees(yaw) * 100;
+            if (yaw_sensor < 0)
+                yaw_sensor += 36000;
+        }
+    }
 }
 
 void AP_AHRS_NavEKF::reset(bool recover_eulers)
 {
     AP_AHRS_DCM::reset(recover_eulers);
+    if (ekf_started) {
+        EKF.InitialiseFilter();        
+    }
 }
 
 // reset the current attitude, used on new IMU calibration
 void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
 {
     AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
+    if (ekf_started) {
+        EKF.InitialiseFilter();        
+    }
 }
 
 // dead-reckoning support
 bool AP_AHRS_NavEKF::get_position(struct Location &loc)
 {
+    if (ekf_started && _ekf_use) {
+        if (EKF.getLLH(loc)) {
+            return true;
+        }
+    }
     return AP_AHRS_DCM::get_position(loc);
 }
 
-// status reporting of estimated error
+// status reporting of estimated errors
 float AP_AHRS_NavEKF::get_error_rp(void)
 {
     return AP_AHRS_DCM::get_error_rp();
@@ -75,7 +118,12 @@ float AP_AHRS_NavEKF::get_error_yaw(void)
 // return a wind estimation vector, in m/s
 Vector3f AP_AHRS_NavEKF::wind_estimate(void)
 {
-    return AP_AHRS_DCM::wind_estimate();
+    if (!ekf_started || !_ekf_use) {
+        return AP_AHRS_DCM::wind_estimate();
+    }
+    Vector3f wind;
+    EKF.getWind(wind);
+    return wind;
 }
 
 // return an airspeed estimate if available. return true
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index e4e8b99e7c..bbc821083f 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -35,7 +35,9 @@ public:
     AP_AHRS_NavEKF(AP_InertialSensor &ins, GPS *&gps, AP_Baro &baro) :
         AP_AHRS_DCM(ins, gps),
         EKF(this, baro),
-        _baro(baro)
+        _baro(baro),
+        ekf_started(false),
+        startup_delay_ms(5000)
         {
         }
 
@@ -74,6 +76,10 @@ public:
 private:
     NavEKF EKF;
     AP_Baro &_baro;
+    bool ekf_started;
+    Matrix3f _dcm_matrix;
+    const uint16_t startup_delay_ms;
+    uint32_t start_time_ms;
 };
 #endif