Copter: straight line waypoints accept terrain

auto_wp_start calls AC_WPNav's new set_wp_destination which accepts a
Location class allow altitude to be set as above-terrain or even an
absolute altitude

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();

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);

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();