// do_user_takeoff_start - initialises waypoint controller to implement take-off bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; // initialise wpnav destination Location target_loc = copter.current_loc; target_loc.set_alt_cm(final_alt_above_home, Location::ALT_FRAME_ABOVE_HOME); if (!wp_nav->set_wp_destination(target_loc)) { // failure to set destination can only be because of missing terrain data copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); // get initial alt for WP_NAVALT_MIN auto_takeoff_set_start_alt(); return true; }
// auto_takeoff_start - initialises waypoint controller to implement take-off void Copter::auto_takeoff_start(const Location& dest_loc) { auto_mode = Auto_TakeOff; // convert location to class Location_Class dest(dest_loc); // set horizontal target dest.lat = current_loc.lat; dest.lng = current_loc.lng; // get altitude target int32_t alt_target; if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); // fall back to altitude above current altitude alt_target = current_loc.alt + dest.alt; } // sanity check target if (alt_target < current_loc.alt) { dest.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); } // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude if (alt_target < 100) { dest.set_alt_cm(100, Location_Class::ALT_FRAME_ABOVE_HOME); } // set waypoint controller target if (!wp_nav.set_wp_destination(dest)) { // failure to set destination can only be because of missing terrain data failsafe_terrain_on_event(); return; } // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); // get initial alt for TKOFF_NAV_ALT auto_takeoff_set_start_alt(); }