#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
Code from: http://python.dronekit.io/guide/taking_off.html
"""

from dronekit import connection, VehicleMode
import time

vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=57600)

def arm_and_takeoff(targetAltitude):
	print "Basic pre-arm checks"
	# Don't try to arm until vehicle is ready
	while not vehicle.is_armable:
		print " Waiting for vehicle to initialise..."
		time.sleep(1)
	
	print "Arming motors"
	# Copter should arm in "GUIDED" mode
	vehicle.mode = VehicleMode("GUIDED")
	vehicle.armed = True

	# Confirm vehicle armed before attempting to take off
	while not vehicle.armed:
		print " Waiting for arming..."
		time.sleep(1)

	print "Taking off!"
	vehicle.simple_takeoff(targetAltitude)

	# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
	#	after Vehicle.simple_takeoff will execute immediately).
	while True:
		print " Altitude: ", vehicle.location.global_relative_frame.alt
		# Break and return from function just below target altitude.
		if vehicle.location.global_relative_frame.alt >= targetAltitude * 0.95:
			print "Reached target altitude"
			break
		time.sleep(1)


if __name__ == '__main__':
	arm_and_takeoff(5)
