From 6781a8d3290d846e9abcb925b7fee0302f5000e2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 21 Feb 2015 10:12:53 +1100
Subject: [PATCH] AP_AHRS: fixed get_position for EKF to use correct relative
 altitude

we need to use the EKF relative height plus the current AHRS home
---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 2435c99fd5..13ce921cef 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -168,7 +168,11 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
 // dead-reckoning support
 bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
 {
-    if (using_EKF() && EKF.getLLH(loc)) {
+    Vector3f ned_pos;
+    if (using_EKF() && EKF.getLLH(loc) && EKF.getPosNED(ned_pos)) {
+        // fixup altitude using relative position from AHRS home, not
+        // EKF origin
+        loc.alt = get_home().alt - ned_pos.z*100;
         return true;
     }
     return AP_AHRS_DCM::get_position(loc);