Copter: guided mode handles terrain alt

ArduCopter/Copter.h

@@ -771,12 +771,13 @@ private:
     bool flip_init(bool ignore_checks);
     void flip_run();
     bool guided_init(bool ignore_checks);
-    void guided_takeoff_start(float final_alt_above_home);
+    bool guided_takeoff_start(float final_alt_above_home);
     void guided_pos_control_start();
     void guided_vel_control_start();
     void guided_posvel_control_start();
     void guided_angle_control_start();
     void guided_set_destination(const Vector3f& destination);
+    bool guided_set_destination(const Location_Class& dest_loc);
     void guided_set_velocity(const Vector3f& velocity);
     void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
     void guided_set_angle(const Quaternion &q, float climb_rate_cms);

ArduCopter/commands_logic.cpp

@@ -809,8 +809,6 @@ bool Copter::verify_yaw()
 // do_guided - start guided mode
 bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
 {
-    Vector3f pos_or_vel;    // target location or velocity
-
     // only process guided waypoint if we are in guided mode
     if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
         return false;
@@ -820,11 +818,12 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
     switch (cmd.id) {
 
         case MAV_CMD_NAV_WAYPOINT:
+        {
             // set wp_nav's destination
-            pos_or_vel = pv_location_to_vector(cmd.content.location);
-            guided_set_destination(pos_or_vel);
-            return true;
+            Location_Class dest(cmd.content.location);
+            return guided_set_destination(dest);
             break;
+        }
 
         case MAV_CMD_CONDITION_YAW:
             do_yaw(cmd);

ArduCopter/control_guided.cpp

@@ -51,20 +51,26 @@ bool Copter::guided_init(bool ignore_checks)
 
 
 // guided_takeoff_start - initialises waypoint controller to implement take-off
-void Copter::guided_takeoff_start(float final_alt_above_home)
+bool Copter::guided_takeoff_start(float final_alt_above_home)
 {
     guided_mode = Guided_TakeOff;
-    
+
     // initialise wpnav destination
-    Vector3f target_pos = inertial_nav.get_position();
-    target_pos.z = pv_alt_above_origin(final_alt_above_home);
-    wp_nav.set_wp_destination(target_pos);
+    Location_Class target_loc = current_loc;
+    target_loc.set_alt(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
+
+    if (!wp_nav.set_wp_destination(target_loc)) {
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        return false;
+    }
 
     // initialise yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);
 
     // clear i term when we're taking off
     set_throttle_takeoff();
+
+    return true;
 }
 
 // initialise guided mode's position controller
@@ -169,6 +175,26 @@ void Copter::guided_set_destination(const Vector3f& destination)
     Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
 }
 
+// sets guided mode's target from a Location object
+// returns false if destination could not be set (probably caused by missing terrain data)
+bool Copter::guided_set_destination(const Location_Class& dest_loc)
+{
+    // ensure we are in position control mode
+    if (guided_mode != Guided_WP) {
+        guided_pos_control_start();
+    }
+
+    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);
+        return false;
+    }
+
+    // log target
+    Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
+    return true;
+}
+
 // guided_set_velocity - sets guided mode's target velocity
 void Copter::guided_set_velocity(const Vector3f& velocity)
 {
@@ -289,7 +315,9 @@ void Copter::guided_takeoff_run()
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    wp_nav.update_wpnav();
+    if (wp_nav.update_wpnav()) {
+        // To-Do: handle failure to update probably caused by missing terrain data
+    }
 
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control.update_z_controller();
@@ -330,7 +358,11 @@ void Copter::guided_pos_control_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();

ArduCopter/takeoff.cpp

@@ -38,9 +38,11 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
 
         switch(control_mode) {
             case GUIDED:
-                set_auto_armed(true);
-                guided_takeoff_start(takeoff_alt_cm);
-                return true;
+                if (guided_takeoff_start(takeoff_alt_cm)) {
+                    set_auto_armed(true);
+                    return true;
+                }
+                return false;
             case LOITER:
             case POSHOLD:
             case ALT_HOLD: