Example: Simple Go To (Copter)

This example demonstrates how to arm and launch a Copter in GUIDED mode, travel to a number of waypoints, and then return to the home location. It uses Vehicle.commands.takeoff(), Vehicle.commands.goto() and Vehicle.mode.

The locations used are centred 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.

Running the example

The vehicle and DroneKit should be set up as described in Getting Started.

If you’re using a simulated vehicle, remember to disable arming checks so that the example can run.

Once MAVProxy is running and the API is loaded, you can start the example by typing: api start simple_goto.py.

Note

The command above assumes you started the MAVProxy prompt in a directory containing the example script. If not, you will have to specify the full path to the script (something like): api start /home/user/git/dronekit-python/examples/simple_goto/simple_goto.py.

Tip

It is more interesting to watch the example above 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).

On the MAVProxy console you should see (something like):

MAV> api start simple_goto.py
STABILIZE> Basic pre-arm checks
Arming motors
 Waiting for arming...
 Waiting for arming...
APM: ARMING MOTORS
APM: GROUND START
 Waiting for arming...
 Waiting for arming...
GUIDED> Mode GUIDED
APM: Initialising APM...
Got MAVLink msg: COMMAND_ACK {command : 400, result : 0}
Waiting for arming...
ARMED
Taking off!
 Altitude:  0.0
Got MAVLink msg: COMMAND_ACK {command : 22, result : 0}
 Altitude:  0.10000000149
 Altitude:  0.620000004768
 ...
 Altitude:  19.25
Reached target altitude
Going to first point...
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Going to second point...
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Returning to Launch
APIThread-0 exiting...

Tip

If you get stuck in Waiting for arming... it is very likely that the vehicle did not pass all pre-arm checks. On a real device you can view the controller LEDs to determine likely issues. On the Simulator console you can disable the checks if needed:

STABILIZE>param load ../Tools/autotest/copter_params.parm
STABILIZE>param set ARMING_CHECK 0

How does it work?

The code has three distinct sections: arming and takeoff, flight to a specified location, and return-to-home.

Takeoff

To launch Copter you need to set the mode to GUIDED, arm the vehicle, and then call Vehicle.commands.takeoff(). The takeoff code in this example is explained in the guide topic Taking Off.

Flying to a point - Goto

The vehicle is already in GUIDED mode, so to send it to a certain point we just need to call Vehicle.commands.goto() with the target location, and then flush() the command:

point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()

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

Without some sort of “wait” the next command would be executed immediately. In this example we just sleep for 30 seconds - a good opportunity to observe the vehicle’s movement on a map.

RTL - Return to launch

To return to the home position and land, we set the mode to RTL:

vehicle.mode    = VehicleMode("RTL")
vehicle.flush()

Source code

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

"""
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)

The example demonstrates how to arm and takeoff in Copter and how to navigate to 
points using Vehicle.commands.goto.

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

import time
from droneapi.lib import VehicleMode, Location
from pymavlink import mavutil

api = local_connect()
vehicle = api.get_vehicles()[0]

def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """

    print "Basic pre-arm checks"
    # Don't let the user try to fly autopilot is booting
    if vehicle.mode.name == "INITIALISING":
        print "Waiting for vehicle to initialise"
        time.sleep(1)
    while vehicle.gps_0.fix_type < 2:
        print "Waiting for GPS...:", vehicle.gps_0.fix_type
        time.sleep(1)
		
    print "Arming motors"
    # Copter should arm in GUIDED mode
    vehicle.mode    = VehicleMode("GUIDED")
    vehicle.armed   = True
    vehicle.flush()

    while not vehicle.armed and not api.exit:
        print " Waiting for arming..."
        time.sleep(1)

    print "Taking off!"
    vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
    vehicle.flush()

    # Wait until the vehicle reaches a safe height before processing the goto (otherwise the command 
    #  after Vehicle.commands.takeoff will execute immediately).
    while not api.exit:
        print " Altitude: ", vehicle.location.alt
        if vehicle.location.alt>=aTargetAltitude*0.95: #Just below target, in case of undershoot.
            print "Reached target altitude"
            break;
        time.sleep(1)

arm_and_takeoff(20)


print "Going to first point..."
point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()

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

print "Going to second point..."
point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()

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

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