diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index ca31814d8b..f851989505 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -166,6 +166,7 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
     float track_length_pct = _track_desired/_track_length;
     _target.x = _origin.x + _pos_delta.x * track_length_pct;
     _target.y = _origin.y + _pos_delta.y * track_length_pct;
+    _target.z = _destination.z;
 }
 
 /// get_distance_to_destination - get horizontal distance to destination in cm
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 8bf146a3a9..a4088e9aa0 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -34,6 +34,9 @@ public:
     /// get_loiter_target - get loiter target as position vector (from home in cm)
     Vector3f get_loiter_target() { return _target; }
 
+    /// get_target_alt - get target altitude from home in cm
+    float get_target_alt() { return _target.z; }
+
     /// set_loiter_target in cm from home
     void set_loiter_target(const Vector3f& position) { _target = position; }