diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 92f702e999da4ed8896a755c48002dce447c42fd..1d700d657629138cd864e176591efa4940136370 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -38,7 +38,7 @@ static void calc_wp_distance()
     // get target from loiter or wpinav controller
     if (control_mode == LOITER || control_mode == CIRCLE) {
         wp_distance = wp_nav.get_loiter_distance_to_target();
-    }else if (control_mode == AUTO) {
+    }else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_distance = wp_nav.get_wp_distance_to_destination();
     }else{
         wp_distance = 0;
@@ -51,7 +51,7 @@ static void calc_wp_bearing()
     // get target from loiter or wpinav controller
     if (control_mode == LOITER || control_mode == CIRCLE) {
         wp_bearing = wp_nav.get_loiter_bearing_to_target();
-    } else if (control_mode == AUTO) {
+    } else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_bearing = wp_nav.get_wp_bearing_to_destination();
     } else {
         wp_bearing = 0;