diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index ecffc2d581..3fa9fa7849 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -557,6 +557,13 @@ void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
 	_pos_control.get_stopping_point_xy(stopping_point);
 }
 
+/// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
+void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
+{
+    _pos_control.get_stopping_point_xy(stopping_point);
+    _pos_control.get_stopping_point_z(stopping_point);
+}
+
 /// advance_wp_target_along_track - move target location along track from origin to destination
 bool AC_WPNav::advance_wp_target_along_track(float dt)
 {
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 45ac6cc686..1c9911f354 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -168,6 +168,7 @@ public:
     /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
     ///		results placed in stopping_position vector
     void get_wp_stopping_point_xy(Vector3f& stopping_point) const;
+    void get_wp_stopping_point(Vector3f& stopping_point) const;
 
     /// get_wp_distance_to_destination - get horizontal distance to destination in cm
     float get_wp_distance_to_destination() const;