Example: Flight Replay

This is an interesting demo that uses our web API to query raw flight data from a particular flight.

Starting the demo

In this case, we pick some public flight from Droneshare:

../_images/flight_replay_example.png

You’ll notice that the mission number for this flight is 101.

Now we’ll launch flight_replay.py (/examples/flight_replay/flight_replay.py) and ask it to try and ‘replay’ mission 101. It will ask the web server for representative points from the flight, parse the JSON response and use that data to generate 100 waypoints we would like our vehicle to hit. For safety rather than using the altitude from the original flight we instead ask our vehicle to fly at a height of 30 meters.

One possible use of some variant of this tool to replay your old flights at your regular test field.

STABILIZE> api start flight_replay.py 101
STABILIZE> JSON downloaded...
Genrating 95 waypoints from replay...
APIThread-1 exiting...
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
Sent waypoint 0 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 0, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 45.7379052, y : 126.6273574, z : 30.0}
Sent waypoint 1 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 1, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 45.7378905, y : 126.6273609, z : 30.0}
Sent waypoint 2 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 2, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0,
...
Sent waypoint 92 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 92, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 45.737971, y : 126.6274908, z : 30.0}
Sent waypoint 93 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 93, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 45.738018, y : 126.6275664, z : 30.0}
Sent waypoint 94 : MISSION_ITEM {target_system : 1, target_component : 1, seq : 94, frame : 3, command : 16, current : 0, autocontinue : 0, param1 : 0, param2 : 0, param3 : 0, param4 : 0, x : 45.7380429, y : 126.6275067, z : 30.0}
Sent all 95 waypoints
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0}
APM: flight plan received

How it works

Getting the points

The following simple function asks for the droneshare flight data:

def download_messages(mission_id, max_freq = 1.0):
    """Download a public mission from droneshare (as JSON)"""
    f = urllib.urlopen("%s/api/v1/mission/%s/messages.json?max_freq=%s&api_key=%s" % (api_server, mission_id, max_freq, api_key))
    j = json.load(f, object_hook=_decode_dict)
    f.close()
    return j

Some comments:

  • max_freq is used to throttle the messages found in the raw flight data to a lower message rate
  • _decode_dict is a utility function found on stack overflow which extracts usable strings from unicode encoded JSON (see flight_replay.py for its implementation).

Setting the new waypoints

We generate up to 100 waypoints for the vehicle with the following code:

print "Generating %s waypoints from replay..." % len(messages)
cmds = v.commands
cmds.clear()
v.flush()
for i in xrange(0, len(messages)):
    pt = messages[i]
    lat = pt['lat']
    lon = pt['lon']
    # To prevent accidents we don't trust the altitude in the original flight, instead
    # we just put in a conservative cruising altitude.
    altitude = 30.0 # pt['alt']
    cmd = Command( 0,
                   0,
                   0,
                   mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                   mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                   0, 0, 0, 0, 0, 0,
                   lat, lon, altitude)
    cmds.add(cmd)
v.flush()

Next we’ll work with existing Linux services (gpsd) to add a new drone based feature called Follow Me.

Source code

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

"""
flight_replay.py: 

An example of talking to Droneshare to receive a past flight, and then 'replaying' 
that flight by sending waypoints to a vehicle.

Start this example as shown below, specifying the 
missionid (a numeric mission number from a public droneshare flight):

    api start flight_replay.py <missionid>

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

from droneapi.lib import Command
from pymavlink import mavutil
import json, urllib, math

api_server = "https://api.3drobotics.com"
api_key = "a8948c11.9e44351f6c0aa7e3e2ff6d00b14a71c5"

# First get an instance of the API endpoint
api = local_connect()
# Get our vehicle - when running with MAVProxy it only knows about one vehicle (for now)
v = api.get_vehicles()[0]


def _decode_list(data):
    """A utility function for decoding JSON strings from unicode"""
    rv = []
    for item in data:
        if isinstance(item, unicode):
            item = item.encode('utf-8')
        elif isinstance(item, list):
            item = _decode_list(item)
        elif isinstance(item, dict):
            item = _decode_dict(item)
        rv.append(item)
    return rv

def _decode_dict(data):
    """A utility function for decoding JSON strings from Unicode"""
    rv = {}
    for key, value in data.iteritems():
        if isinstance(key, unicode):
            key = key.encode('utf-8')
        if isinstance(value, unicode):
            value = value.encode('utf-8')
        elif isinstance(value, list):
            value = _decode_list(value)
        elif isinstance(value, dict):
            value = _decode_dict(value)
        rv[key] = value
    return rv

def download_messages(mission_id, max_freq = 1.0):
    """Download a public mission from droneshare (as JSON)"""
    f = urllib.urlopen("%s/api/v1/mission/%s/messages.json?max_freq=%s&api_key=%s" % (api_server, mission_id, max_freq, api_key))
    j = json.load(f, object_hook=_decode_dict)
    f.close()
    return j

def replay_mission(payload):
    """Given mission JSON, set a series of wpts approximating the previous flight"""
    # Pull out just the global position msgs
    messages = payload['messages']
    messages = filter(lambda obj: obj['typ'] == 'MAVLINK_MSG_ID_GLOBAL_POSITION_INT', messages)
    messages = map(lambda obj: obj['fld'], messages)

    # Shrink the # of pts to be lower than the max # of wpts allowed by vehicle
    num_points = len(messages)
    max_points = 99
    if num_points > max_points:
        step = int(math.ceil((float(num_points) / max_points)))
        shorter = [messages[i] for i in xrange(0, num_points, step)]
        messages = shorter

    print "Generating %s waypoints from replay..." % len(messages)
    cmds = v.commands
    cmds.clear()
    v.flush()
    for i in xrange(0, len(messages)):
        pt = messages[i]
        lat = pt['lat']
        lon = pt['lon']
        # To prevent accidents we don't trust the altitude in the original flight, instead
        # we just put in a conservative cruising altitude.
        altitude = 30.0 # pt['alt']
        cmd = Command( 0,
                       0,
                       0,
                       mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                       mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                       0, 0, 0, 0, 0, 0,
                       lat, lon, altitude)
        cmds.add(cmd)
    v.flush()

if len(local_arguments) != 1:
    print 'Error: usage "api start flight_replay.py <missionid>"'
else:
    # Now download the vehicle waypoints
    cmds = v.commands
    cmds.wait_valid()

    mission_id = int(local_arguments[0])
    max_freq = 0.1
    json = download_messages(mission_id, max_freq)
    print "JSON downloaded..."
    replay_mission(json)