Simple Goto

This example demonstrates how to arm and launch a Copter in GUIDED mode, travel towards a number of target points, and then return to the home location.

To execute this code:

python goto.py

from droneki    t import connect, VehicleMode, LocationGlobalRelative
import time

#Set up option parsing to get connection string
import argparse  
parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.')
parser.add_argument('--connect', 
                   help="Vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()

connection_string = args.connect
sitl = None


#Start SITL if no connection string specified
if not connection_string:
    import dronekit_sitl
    sitl = dronekit_sitl.start_default()
    connection_string = sitl.connection_string()

# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)

def arm_and_takeoff(aTargetAltitude):

    print "Basic pre-arm checks"

    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(aTargetAltitude) # Take off to target altitude

    # Wait until the vehicle reaches a safe height
    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>=aTargetAltitude*0.95:
            print "Reached target altitude"
            break
        time.sleep(1)

arm_and_takeoff(1)

print "Set default/target airspeed to 3"
vehicle.airspeed = 3

print "Go to the first point."

point1 = LocationGlobalRelative(-20.669880973,103.42261223, 1)
vehicle.simple_goto(point1)

time.sleep(5)

print "Go to the second poin (groundspeed set to 10 m/s) ..."
point2 = LocationGlobalRelative(20.43, 103.466772250, 1)
vehicle.simple_goto(point2, groundspeed=10)

time.sleep(5)

print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")

#Close vehicle object before exiting script
print "Close vehicle object"
vehicle.close()

# Shut down simulator if it was started.
if sitl is not None:
    sitl.stop()