AP_AHRS_NavEKF: overload get_accel_ef and get_accel_ef_blended functions

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);

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;
 };