Example: Basic Mission

This example demonstrates the basic mission operations provided by DroneKit-Python, including: downloading missions from the vehicle, clearing missions, creating mission commands and uploading them to the vehicle, monitoring the current active command, and changing the active command.

The guide topic Missions (AUTO Mode) provides more detailed explanation of how the API should be used.

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 mission_basic.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/mission_basic/mission_basic.py.

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

MAV> api start mission_basic.py
STABILIZE> Clear the current mission
       Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Requesting 0 waypoints t=Wed Jul 29 21:27:58 2015 now=Wed Jul 29 21:27:58 2015
Create a new mission
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Sent waypoint 0 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 0, frame : 0, command : 16, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.3632621765, y : 149.165237427, z : 584.0}
Sent waypoint 1 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 1, frame : 3, command : 22, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 0, y : 0, z : 10}
Sent waypoint 2 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 2, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : -35.3628118424, y : 149.164679124, z : 11}
Sent waypoint 3 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 3, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : -35.3628118424, y : 149.165780676, z : 12}
Sent waypoint 4 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 4, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : -35.3637101576, y : 149.165780676, z : 13}
Sent waypoint 5 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 5, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : -35.3637101576, y : 149.164679124, z : 14}
Sent all 6 waypoints
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
APM: flight plan received
Basic pre-arm checks
Arming motors
 Waiting for arming...
Got MAVLink msg: COMMAND_ACK {command : 11, result : 0}
 Waiting for arming...
APM: ARMING MOTORS
APM: GROUND START
 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}
GPS lock at 0 meters
 Altitude:  0.10000000149
 ...
 Altitude:  8.84000015259
 Altitude:  9.60999965668
Reached target altitude
Starting mission
Got MAVLink msg: COMMAND_ACK {command : 11, result : 0}
waypoint 1
waypoint 2
AUTO> Mode AUTO
Distance to waypoint (2): 79.3138466142
Distance to waypoint (2): 79.1869592549
Distance to waypoint (2): 77.8436803794
...
Distance to waypoint (2): 20.7677087176
Distance to waypoint (2): 15.4592692026
APM: Reached Command #2
waypoint 3
Distance to waypoint (3): 115.328425048
Skipping to Waypoint 4 when reach waypoint 3
waypoint 4
Distance to waypoint (4): 152.376018911
Distance to waypoint (4): 154.882233097
...
Distance to waypoint (4): 20.4052797291
Distance to waypoint (4): 15.0592597507
APM: Reached Command #4
waypoint 5
Distance to waypoint (5): 114.450267446
Exit 'standard' mission when start heading to final waypoint (5)
Return to launch
APIThread-0 exiting...

How does it work?

The source code is relatively self-documenting, and most of its main operations are explained in the guide topic Missions (AUTO Mode) .

In overview, the example first calls clear_mission() to clear the current mission and then creates and uploads a new mission using adds_square_mission(vehicle.location,50). This function defines a mission with a takeoff command and four waypoints arranged in a square around the central position.

After taking off (in guided mode using the takeoff() function) the example starts the mission by setting the mode to AUTO:

print "Starting mission"
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
vehicle.flush()

The progress of the mission is monitored in a loop. The convenience function distance_to_current_waypoint() gets the distance to the next waypoint and Vehicle.commands.next gets the value of the next command.

We also show how to move to a specified command using Vehicle.commands.next (note how we skip the third command below):

while True:
    nextwaypoint =vehicle.commands.next
    if nextwaypoint  > 1:
        print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint())
    if nextwaypoint ==3: #Skip to next waypoint
        print 'Skipping to Waypoint 4 when reach waypoint 3'
        vehicle.commands.next=4
    if nextwaypoint ==5: #Skip to next waypoint
        print "Exit 'standard' mission when start heading to final waypoint (5)"
        break;
    time.sleep(1)

When the vehicle starts the 5th command the loop breaks and the mode is set to RTL (return to launch).

Known issues

This example works around the known issues in the API. Provided that the vehicle is connected and able to arm, it should run through to completion.

Source code

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

"""
mission_basic.py: Example demonstrating basic mission operations including creating, clearing and monitoring missions.

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

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

# Connect to API provider and get vehicle
api = local_connect()
vehicle = api.get_vehicles()[0]


def get_location_metres(original_location, dNorth, dEast):
    """
    Returns a Location object containing the latitude/longitude `dNorth` and `dEast` metres from the 
    specified `original_location`. The returned Location has the same `alt and `is_relative` values 
    as `original_location`.

    The function is useful when you want to move the vehicle around specifying locations relative to 
    the current vehicle position.
    The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.
    For more information see:
    http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters
    """
    earth_radius=6378137.0 #Radius of "spherical" earth
    #Coordinate offsets in radians
    dLat = dNorth/earth_radius
    dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))

    #New position in decimal degrees
    newlat = original_location.lat + (dLat * 180/math.pi)
    newlon = original_location.lon + (dLon * 180/math.pi)
    return Location(newlat, newlon,original_location.alt,original_location.is_relative)


def get_distance_metres(aLocation1, aLocation2):
    """
    Returns the ground distance in metres between two Location objects.

    This method is an approximation, and will not be accurate over large distances and close to the 
    earth's poles. It comes from the ArduPilot test code: 
    https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py
    """
    dlat = aLocation2.lat - aLocation1.lat
    dlong = aLocation2.lon - aLocation1.lon
    return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5



def distance_to_current_waypoint():
    """
    Gets distance in metres to the current waypoint. 
    It returns None for the first waypoint (Home location).
    """
    nextwaypoint=vehicle.commands.next
    if nextwaypoint ==1:
        return None
    missionitem=vehicle.commands[nextwaypoint]
    lat=missionitem.x
    lon=missionitem.y
    alt=missionitem.z
    targetWaypointLocation=Location(lat,lon,alt,is_relative=True)
    distancetopoint = get_distance_metres(vehicle.location, targetWaypointLocation)
    return distancetopoint


def clear_mission():
    """
    Clear the current mission.
    """
    cmds = vehicle.commands
    vehicle.commands.clear()
    vehicle.flush()

    # After clearing the mission you MUST re-download the mission from the vehicle 
    # before vehicle.commands can be used again
    # (see https://github.com/dronekit/dronekit-python/issues/230)
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_valid()
    

def download_mission():
    """
    Download the current mission from the vehicle.
    """
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_valid() # wait until download is complete.



def adds_square_mission(aLocation, aSize):
    """
    Adds a takeoff command and four waypoint commands to the current mission. 
    The waypoints are positioned to form a square of side length 2*aSize around the specified location (aLocation).

    The function assumes vehicle.commands matches the vehicle mission state 
    (you must have called download at least once in the session and after clearing the mission)
    """	
    # Add the commands. The meaning/order of the parameters is documented in the Command class.
    cmds = vehicle.commands
    #Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))

    #Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
    point1 = get_location_metres(aLocation, aSize, -aSize)
    point2 = get_location_metres(aLocation, aSize, aSize)
    point3 = get_location_metres(aLocation, -aSize, aSize)
    point4 = get_location_metres(aLocation, -aSize, -aSize)
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 11))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 12))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 13))
    cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14))

    # Send commands to vehicle.
    vehicle.flush() 


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)


print 'Clear the current mission'
clear_mission()

print 'Create a new mission'
adds_square_mission(vehicle.location,50)
time.sleep(2)  # This is here so that mission being sent is displayed on console


# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently).
arm_and_takeoff(10)

print "Starting mission"
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
vehicle.flush()

# Monitor mission. 
# Demonstrates getting and setting the command number 
# Uses distance_to_current_waypoint(), a convenience function for finding the 
#   distance to the next waypoint.

while True:
    nextwaypoint=vehicle.commands.next
    if nextwaypoint > 1:
        print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint())
    if nextwaypoint==3: #Skip to next waypoint
        print 'Skipping to Waypoint 4 when reach waypoint 3'
        vehicle.commands.next=4
    if nextwaypoint==5: #Skip to next waypoint
        print "Exit 'standard' mission when start heading to final waypoint (5)"
        break;
    time.sleep(1)

print 'Return to launch'
vehicle.mode = VehicleMode("RTL")
vehicle.flush()  # Flush to ensure changes are sent to autopilot