MAVLink Protocol: Waypoints
During the EA-DDDAS deployment this summer we'll be flying a Tempest with a PixHawk autopilot on-board. The PixHawk uses the well-known MAVLink protocol released by Lorenz Meier over at ETH Zürich. The protocol was designed to be language and (mostly) hardware agnostic which is great for portability but this means there isn't much existing framework to write your own simple programs to interface with the PixHawk. While the recently refreshed DroneKit released by 3DR includes a Python implementation (DroneAPI), it only runs as a MAVProxy module. This can be an issue if the code is running on a small supervisory computer like a Raspberry Pi because MAVProxy isn't terribly efficient. We prefer to use MAVProxy as a learning tool that provides a starting point to implementing our own PyMAVLink solution.
We've been developing a Python class for EA-DDDAS that implements a basic waypoint send functionality. Through mavutil.py and mavwp.py, the PyMAVLink library provides the required commands to form and send waypoints. Before diving into some code, it's important to understand how MAVLink accepts a set of waypoints. The sender intializes the request by sending a WAYPOINT_COUNT(N) message to the PixHawk. N represents the number of waypoints that will be sent. The PixHawk responds in turn by asking for the first waypoint through a MISSION_REQUEST(WP) message where WP is the waypoint index it's going to wait for. This process goes back and forth until the PixHawk receives the Nth waypoint and sends an ACK command to tell everyone involved that the transaction is finished.
Here is what it looks like in Python (connected to the ArduPilot SITL):
The code assumes you have a three tuples called "lat", "lon", and "alt" which are "N" long.
from pymavlink import mavutil, mavwp
master = mavutil.mavlink_connection('udp:localhost:14551')
master.wait_heartbeat(blocking=True)
wp = mavwp.MAVWPLoader()
seq = 1
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
radius = 10
for i in range(N):
wp.add(mavutil.mavlink.MAVLink_mission_item_message(master.target_system,
master.target_component,
seq,
frame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, 0, 0, radius, 0, 0,
lat[i],lon[i],alt[i]))
seq += 1
master.waypoint_clear_all_send()
master.waypoint_count_send(wp.count())
for i in range(wp.count()):
msg = master.recv_match(type=['MISSION_REQUEST'],blocking=True)
master.mav.send(wp.wp(msg.seq))
print 'Sending waypoint {0}'.format(msg.seq)
The basic procedure is as follows:
1. Establish a UDP connection to the aircraft
2. Listen for a heartbeat to make sure we're receiving MAVLink messages
3. Create a waypoint (wp) object and populate it with lat,lon,altitudes
4. Send a clear all waypoints command to the PixHawk
5. Send a MISSION_COUNT(N) to the PixHawk
6. Listen for MISSION_REQUEST messages and sent the appropriate waypoint
7. Repeat step 6 until complete
Whew! PyMAVLink is a great library, but it's still not the easiest to use. Something to note is that MAVLink has two released versions of the protocol: 0.9 and 1.0. This code assumes v1.0 and uses the "MISSION_REQUEST" message. In v0.9, this would be "WAYPOINT_REQUEST".
Cheers for now!