Example: Simple Go To (Copter)

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. It uses Vehicle.simple_takeoff(), Vehicle.simple_goto() and Vehicle.mode.

The target locations are centered around the home location when the Simulated Vehicle is booted; you can edit the latitude and longitude to use more appropriate positions for your own vehicle.

Note

This example will only run on Copter:

  • Plane does not support takeoff in GUIDED mode.
  • Rover will ignore the takeoff command and will then stick at the altitude check.
Setting destination using position and changing speed and ROI

Simple Goto Example: Flight path

Running the example

The example can be run as described in Running the Examples (which in turn assumes that the vehicle and DroneKit have been set up as described in Installing DroneKit).

In summary, after cloning the repository:

  1. Navigate to the example folder as shown:

    cd dronekit-python/examples/simple_goto/
    
  2. You can run the example against a simulator (DroneKit-SITL) by specifying the Python script without any arguments. The example will download SITL binaries if needed, start the simulator, and then connect to it:

    python simple_goto.py
    

    On the command prompt you should see (something like):

    Starting copter simulator (SITL)
    SITL already Downloaded.
    Connecting to vehicle on: tcp:127.0.0.1:5760
    >>> APM:Copter V3.3 (d6053245)
    >>> Frame: QUAD
    >>> Calibrating barometer
    >>> Initialising APM...
    >>> barometer calibration complete
    >>> GROUND START
    Basic pre-arm checks
     Waiting for vehicle to initialise...
     ...
     Waiting for vehicle to initialise...
    Arming motors
     Waiting for arming...
     ...
     Waiting for arming...
    >>> ARMING MOTORS
    >>> GROUND START
     Waiting for arming...
    >>> Initialising APM...
    Taking off!
     Altitude:  0.0
     ...
     Altitude:  7.4
     Altitude:  9.0
     Altitude:  9.65
    Reached target altitude
    Set default/target airspeed to 3
    Going towards first point for 30 seconds ...
    Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...
    Returning to Launch
    Close vehicle object
    

    Tip

    It is more interesting to watch the example run on a map than the console. The topic Connecting an additional Ground Station explains how to set up Mission Planner to view a vehicle running on the simulator (SITL).

  3. You can run the example against a specific connection (simulated or otherwise) by passing the connection string for your vehicle in the --connect parameter.

    For example, to connect to SITL running on UDP port 14550 on your local computer:

    python simple_goto.py --connect 127.0.0.1:14550
    

How does it work?

The code has three distinct sections: arming and takeoff, flight to two locations, and return-to-home.

Takeoff

To launch Copter you need to first check that the vehicle Vehicle.is_armable. Then set the mode to GUIDED, arm the vehicle, and call Vehicle.simple_takeoff(). The takeoff code in this example is explained in the guide topic Taking Off.

Flying to a point - simple_goto

The vehicle is already in GUIDED mode, so to send it to a certain point we just need to call Vehicle.simple_goto() with the target dronekit.LocationGlobalRelative:

# set the default travel speed
vehicle.airspeed=3

point1 = LocationGlobalRelative(-35.361354, 149.165218, 20)
vehicle.simple_goto(point1)

# sleep so we can see the change in map
time.sleep(30)

Tip

Without some sort of “wait” the next command would be executed immediately. In this example we just sleep for 30 seconds before executing the next command.

When moving towards the first point we set the airspeed using the Vehicle.airspeed attribute. For the second point the example specifies the target groundspeed when calling Vehicle.simple_goto()

vehicle.simple_goto(point2, groundspeed=10)

Tip

The script doesn’t report anything during the sleep periods, but you can observe the vehicle’s movement on a ground station map.

RTL - Return to launch

To return to the home position and land, we set the mode to RTL. The vehicle travels at the previously set default speed:

vehicle.mode    = VehicleMode("RTL")

Source code

The full source code at documentation build-time is listed below (current version on Github):

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

"""
© Copyright 2015-2016, 3D Robotics.
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)

Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto.

Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html
"""

from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative


# 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):
    """
    Arms vehicle and fly to aTargetAltitude.
    """

    print("Basic pre-arm checks")
    # Don't try to arm until autopilot 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(aTargetAltitude)  # Take off to target altitude

    # 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 >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            break
        time.sleep(1)


arm_and_takeoff(10)

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

print("Going towards first point for 30 seconds ...")
point1 = LocationGlobalRelative(-35.361354, 149.165218, 20)
vehicle.simple_goto(point1)

# sleep so we can see the change in map
time.sleep(30)

print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...")
point2 = LocationGlobalRelative(-35.363244, 149.168801, 20)
vehicle.simple_goto(point2, groundspeed=10)

# sleep so we can see the change in map
time.sleep(30)

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:
    sitl.stop()