From 9261dfdefbe0f22696f370cd35c822a87f9f4f8b Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 31 Oct 2014 15:07:18 -0700
Subject: [PATCH] AP_AHRS_NavEKF: overload get_accel_ef and
 get_accel_ef_blended functions

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 28 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  8 ++++++++
 2 files changed, 36 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 76d09c37f0..ea7e84a882 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -118,10 +118,38 @@ void AP_AHRS_NavEKF::update(void)
                 _gyro_estimate /= healthy_count;
             }
             _gyro_estimate += _gyro_bias;
+
+            // update _accel_ef_ekf
+            for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
+                if (_ins.get_accel_health(i)) {
+                    _accel_ef_ekf[i] = _dcm_matrix * _ins.get_accel(i);
+                }
+            }
+
+            // update _accel_ef_ekf_blended
+            EKF.getAccelNED(_accel_ef_ekf_blended);
         }
     }
 }
 
+// accelerometer values in the earth frame in m/s/s
+const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const
+{
+    if(!using_EKF()) {
+        return AP_AHRS_DCM::get_accel_ef(i);
+    }
+    return _accel_ef_ekf[i];
+}
+
+// blended accelerometer values in the earth frame in m/s/s
+const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const
+{
+    if(!using_EKF()) {
+        return AP_AHRS_DCM::get_accel_ef_blended();
+    }
+    return _accel_ef_ekf_blended;
+}
+
 void AP_AHRS_NavEKF::reset(bool recover_eulers)
 {
     AP_AHRS_DCM::reset(recover_eulers);
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index d349585e49..69083caa38 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -87,6 +87,12 @@ public:
     // EKF has a better ground speed vector estimate
     Vector2f groundspeed_vector(void);
 
+    const Vector3f &get_accel_ef(uint8_t i) const;
+    const Vector3f &get_accel_ef() const { return get_accel_ef(_ins.get_primary_accel()); };
+
+    // blended accelerometer values in the earth frame in m/s/s
+    const Vector3f &get_accel_ef_blended(void) const;
+
     // set home location
     void set_home(const Location &loc);
 
@@ -112,6 +118,8 @@ private:
     Vector3f _dcm_attitude;
     Vector3f _gyro_bias;
     Vector3f _gyro_estimate;
+    Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
+    Vector3f _accel_ef_ekf_blended;
     const uint16_t startup_delay_ms;
     uint32_t start_time_ms;
 };