diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 70589b778b..449bbfe4e1 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2795,6 +2795,12 @@ void NavEKF::getEulerAngles(Vector3f &euler) const euler = euler - _ahrs->get_trim(); } +// This returns the specific forces in the NED frame +void NavEKF::getAccelNED(Vector3f &accelNED) const { + accelNED = velDotNED; + accelNED.z -= GRAVITY_MSS; +} + // return NED velocity in m/s void NavEKF::getVelNED(Vector3f &vel) const { diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 8c3be28c05..d3ab834fcf 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -102,6 +102,9 @@ public: // return NED velocity in m/s void getVelNED(Vector3f &vel) const; + // This returns the specific forces in the NED frame + void getAccelNED(Vector3f &accelNED) const; + // return body axis gyro bias estimates in rad/sec void getGyroBias(Vector3f &gyroBias) const;