diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 80bf47ff93..3b37823e22 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -717,6 +717,7 @@ private:
     void auto_takeoff_start(float final_alt_above_home);
     void auto_takeoff_run();
     void auto_wp_start(const Vector3f& destination);
+    void auto_wp_start(const Location_Class& dest_loc);
     void auto_wp_run();
     void auto_spline_run();
     void auto_land_start();
diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp
index 47bde85be6..99c1d9280a 100644
--- a/ArduCopter/commands_logic.cpp
+++ b/ArduCopter/commands_logic.cpp
@@ -293,16 +293,14 @@ void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd)
 // do_nav_wp - initiate move to next waypoint
 void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
-    const Vector3f &curr_pos = inertial_nav.get_position();
-    const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
-
     // this will be used to remember the time in millis after we reach or pass the WP.
     loiter_time = 0;
     // this is the delay, stored in seconds
     loiter_time_max = cmd.p1;
 
     // Set wp navigation target
-    auto_wp_start(local_pos);
+    auto_wp_start(Location_Class(cmd.content.location));
+
     // if no delay set the waypoint as "fast"
     if (loiter_time_max == 0 ) {
         wp_nav.set_fast_waypoint(true);
diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp
index 3b5058641f..69fc0713c1 100644
--- a/ArduCopter/control_auto.cpp
+++ b/ArduCopter/control_auto.cpp
@@ -164,6 +164,25 @@ void Copter::auto_wp_start(const Vector3f& destination)
     }
 }
 
+// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
+void Copter::auto_wp_start(const Location_Class& dest_loc)
+{
+    auto_mode = Auto_WP;
+
+    // send target to waypoint controller
+    if (!wp_nav.set_wp_destination(dest_loc)) {
+        // failure to set destination (likely because of missing terrain data)
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        // To-Do: handle failure
+    }
+
+    // initialise yaw
+    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
+    if (auto_yaw_mode != AUTO_YAW_ROI) {
+        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+    }
+}
+
 // auto_wp_run - runs the auto waypoint controller
 //      called by auto_run at 100hz or more
 void Copter::auto_wp_run()
@@ -199,7 +218,11 @@ void Copter::auto_wp_run()
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    wp_nav.update_wpnav();
+    if (!wp_nav.update_wpnav()) {
+        // failures to update probably caused by missing terrain data
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET);
+        // To-Do: handle failure by triggering RTL after 5 seconds with no update?
+    }
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control.update_z_controller();