Example: Mission Import/Export

This example shows how to import and export files in the Waypoint file format.

The commands are first imported from a file into a list and then uploaded to the vehicle. Then the current mission is downloaded from the vehicle and put into a list, and finally saved into (another file).

The example does not show how missions can be modified, but once the mission is in a list, changing the order and content of commands is straightforward.

The guide topic Missions (AUTO Mode) provides information about missions and AUTO mode.

Running the example

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

Once MAVProxy is running and the API is loaded, you can start the example by typing: api start mission_import_export.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 absolute path to the script. For example: api start /home/user/git/dronekit-python/examples/mission_import_export/mission_import_export.py.

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

STABILIZE> api start mission_import_export.py
STABILIZE>
Upload mission from a file: mpmission.txt
Reading mission from file: mpmission.txt

STABILIZE>  Import line: 0      1       0       16      0       0       0       0       -35.360500      149.172363      747.000000      1

 Import line: 1 0       3       22      0.000000        0.000000        0.000000        0.000000     -35.359831 149.166334      100.000000      1


 Import line: 2 0       3       16      0.000000        0.000000        0.000000        0.000000     -35.363489 149.167213      100.000000      1

 Import line: 3 0       3       16      0.000000        0.000000        0.000000        0.000000     -35.355491 149.169595      100.000000      1

 Import line: 4 0       3       16      0.000000        0.000000        0.000000        0.000000     -35.355071 149.175839      100.000000      1

 Import line: 5 0       3       113     0.000000        0.000000        0.000000        0.000000     -35.362666 149.178715      22222.000000    1
 Import line: 6 0       3       115     2.000000        22.000000       1.000000        3.000000     0.000000   0.000000        0.000000        1

 Import line: 7 0       3       16      0.000000        0.000000        0.000000        0.000000     0.000000   0.000000        0.000000        1

Clear mission
Requesting 9 waypoints t=Wed Jul 29 20:12:17 2015 now=Wed Jul 29 20:12:17 2015
ClearCount: 0
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Requesting 0 waypoints t=Wed Jul 29 20:12:17 2015 now=Wed Jul 29 20:12:17 2015
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 : 0, command : 16, current : 1, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.3605, y : 149.172363, z : 747.0}
Sent waypoint 2 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 2, frame : 3, command : 22, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.359831, y : 149.166334, z : 100.0}
Sent waypoint 3 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 3, frame : 3, command : 16, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.363489, y : 149.167213, z : 100.0}
Sent waypoint 4 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 4, frame : 3, command : 16, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.355491, y : 149.169595, z : 100.0}
Sent waypoint 5 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 5, frame : 3, command : 16, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.355071, y : 149.175839, z : 100.0}
Sent waypoint 6 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 6, frame : 3, command : 113, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : -35.362666, y : 149.178715, z : 22222.0}
Sent waypoint 7 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 7, frame : 3, command : 115, current : 0, autocontinue : 1, param1 : 2.0, param2 : 22.0, param3 : 1.0, param4 : 3.0, x : 0.0, y : 0.0, z : 0.0}
Sent waypoint 8 : MISSION_ITEM {target_system : 1, target_component : 0, seq : 8, frame : 3, command : 16, current : 0, autocontinue : 1, param1 : 0.0, param2 : 0.0, param3 : 0.0, param4 : 0.0, x : 0.0,y : 0.0, z : 0.0}
Sent all 9 waypoints
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
APM: flight plan received

Save mission from Vehicle to file: exportedmission.txt
Requesting 9 waypoints t=Wed Jul 29 20:12:18 2015 now=Wed Jul 29 20:12:18 2015
APIThread-1 exiting...

How does it work?

The source code is largely self-documenting.

More information about the functions can be found in the guide at Load a mission from a file and Save a mission to a file.

Known issues

This example works around known issues in the API:

  • A time.sleep(1) has been placed between uploading the mission to the vehicle (from the file) and downloading the mission. This is to avoid the race condition where the mission being downloaded has not yet successfully uploaded to the vehicle. This race condition (probably) shouldn’t exist because the mission is flushed to the Vehicle - see Race condition when updating and fetching commands

Source code

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

"""
mission_import_export.py: 

This example demonstrates how to import and export files in the Waypoint file format 
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format). The commands are imported
into a list, and can be modified before saving and/or uploading.

Documentation is provided at http://python.dronekit.io/examples/mission_import_export.html
"""

import time
import math
from droneapi.lib import Command
from pymavlink import mavutil

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


def readmission(aFileName):
    """
    Load a mission from a file into a list. The mission definition is in the Waypoint file
    format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).

    This function is used by upload_mission().
    """
    print "Reading mission from file: %s\n" % aFileName
    cmds = vehicle.commands
    missionlist=[]
    with open(aFileName) as f:
        for i, line in enumerate(f):
            if i==0:
                if not line.startswith('QGC WPL 110'):
                    raise Exception('File is not supported WP version')
            else:
                print ' Import line: %s' % line
                linearray=line.split('\t')
                ln_index=int(linearray[0])
                ln_currentwp=int(linearray[1])
                ln_frame=int(linearray[2])
                ln_command=int(linearray[3])
                ln_param1=float(linearray[4])
                ln_param2=float(linearray[5])
                ln_param3=float(linearray[6])
                ln_param4=float(linearray[7])
                ln_param5=float(linearray[8])
                ln_param6=float(linearray[9])
                ln_param7=float(linearray[10])	
                ln_autocontinue=int(linearray[11].strip())		
                cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
                missionlist.append(cmd)
    return missionlist


def upload_mission(aFileName):
    """
    Upload a mission from a file. 
    """
    missionlist = readmission(aFileName)
    #clear existing mission
    print 'Clear mission'
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_valid()
    cmds.clear()
    vehicle.flush()
    print 'ClearCount: %s' % cmds.count
    #add new mission
    cmds.download()
    cmds.wait_valid()
    for command in missionlist:
        cmds.add(command)
    vehicle.flush()	


def download_mission():
    """
    Downloads the current mission and returns it in a list.
    It is used in save_mission() to get the file information to save.
    """
    missionlist=[]
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_valid()
    for cmd in cmds[1:]:  #skip first item as it is home waypoint.
        missionlist.append(cmd)
    return missionlist

def save_mission(aFileName):
    """
    Save a mission in the Waypoint file format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
    """
    missionlist = download_mission()
    output='QGC WPL 110\n'
    for cmd in missionlist:
        commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
        output+=commandline
    with open(aFileName, 'w') as file_:
        file_.write(output)    


import_mission_filename = 'mpmission.txt'
export_mission_filename = 'exportedmission.txt'

print "\nUpload mission from a file: %s" % import_mission_filename		
upload_mission(import_mission_filename)

time.sleep(1)

print "\nSave mission from Vehicle to file: %s" % export_mission_filename
save_mission(export_mission_filename)