Uploading a waypoint and interfacing the Pixhawk to an Arduino using Mavlink

I wrote this post to outline the Mavlink Mission Protocol for uploading a new flight plan to the Autopilot using the Arduino Mavlink library.

A good starting point to understanding how to use the files included in the Arduino Mavlink library is provided here:

Understanding the Mavlink Mission Protocol

In order to successfully upload a new flightplan to a Pixhawk using an Arduino one needs to understand the Mavlink Mission Protocol. See: https://mavlink.io/en/services/mission.html#mission_types

In the image above, the mission protocol is explained in visual form.

In more detail (copied from the link above), the sequence of operations are:

  1. GCS (client) sends MISSION_COUNT including the number of mission items to be uploaded (count)
  2. A timeout must be started for the GCS to wait on the response from Drone (MISSION_REQUEST_INT) .
  3. Drone (server) receives the message, and prepares to upload mission items.
  4. Drone responds with MISSION_REQUEST_INT requesting the first mission item (seq==1).
  5. GCS receives MISSION_REQUEST_INT and responds with the requested mission item in a MISSION_ITEM_INT message.
  6. Drone and GCS repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle, iterating seq until all items are uploaded.
  7. For the last mission item, the drone responds with MISSION_ACK with the result of the operation result: type (MAV_MISSION_RESULT):
    • On success, type must be set to MAV_MISSION_ACCEPTED.
    • On failure, type must set to MAV_MISSION_ERROR or some other error code.
  8. GCS receives MISSION_ACK: If MAV_MISSION_ACCEPTED the operation is complete. If an error, the transaction fails but may be retried.

This high level understanding of how the Mavlink Mission Protocol flows is KEY in understanding how the Pixhawk manages a flight plan (or way-point) sent by the Arduino. Without following the steps shown above, the Pixhawk will not successfully accept a new flight plan/way-point.

The more you know…

The implementation status for the PX4 Pixhawk (at time of writing):

  1. Flight plan missions:
    • upload, download, clearing missions, and monitoring progress are supported as defined in this specification.
    • partial upload and partial download are not supported.
  2. Geofence “missions” are supported as defined in this specification.
  3. Rally point “missions” are not supported on PX4.
  4. You can interchange MISSION_ITEM_INT with MISSION_ITEM. The only difference is that MISSION_ITEM_INT encodes the latitude and longitude as integers rather than floats.
  5. It seems that setting the initial HOME way-point in the example below does not seem to affect anything as HOME will be set once the vehicle is armed.

**When first using this library and referencing the Mavlink documentation, MISSION_ITEM_INT was referenced as the way to send way-points to the Pixhawk. However, this file was not included in the Arduino mavlink library. It WAS included in the Mavlink2 version of the library which I then tried to install and work with unsuccessfully. After finding this note in the Mission Protocol documentation, I could have saved myself some time and just used the mavlink_msg_mission_item.h file instead of chasing down a mavlink_msg_mission_item.int file unnecessarily.

Simplifying the protocol

  1. Arduino sends MISSION_COUNT including the number of mission items to be uploaded.
  2. Pixhawk receives the message, responds with MISSION_REQUEST requesting the first mission item.
  3. Arduino receives MISSION_REQUEST and responds with the requested mission item in a MISSION_ITEM message.
  4. Pixhawk and Arduino repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle until all items are uploaded.
  5. For the last mission item, the Pixhawk responds with MISSION_ACK with the result of the operation result: type (MAV_MISSION_RESULT):
    • On success, type must be set to MAV_MISSION_ACCEPTED (0).
    • On failure, type must set to MAV_MISSION_ERROR (1).
  6. Arduino receives MISSION_ACK: If failed or successful, then do something (example code below not given for this step).

Coding the mission protocol

Arduino Mavlink Library files required (located in common directory):

Files to handle sending of mission variables to Pixhawk

  • mavlink_msg_mission_count.h
  • mavlink_msg_mission_item.h

Files to handle receiving messages sent by Pixhawk

  • mavlink_msg_mission_request.h
  • mavlink_msg_mission_ack.h

If we look at the the Mission Protocol, we see that what actually gets the Pixhawk to begin the process of accepting parameters of new way-points is not the mavlink_msg_mission_item.h parameters (which we would expect), but the mavlink_msg_mission_count.h parameters instead. Sending a mavlink_msg_mission_count message lets the Pixhawk know that we want to upload some new way-points and how many to expect. Without initiating the protocol with this Mavlink message, the Pixhawk would not know what to do with the sent mission item parameters.

The mission_count() function packs the data we specify in variables and sends it to the Pixhawk – letting it know we want to upload two way-points (with HOME being the first way-point, and another way-point being the first real destination). After this message is sent, the Pixhawk will then send a Mission Request message back to the Arduino specifying the sequence of way-points it is expecting. We then use this message as a trigger to begin the next step in the protocol.

The code snippet above does not account for timeouts and retries and it certainly isn’t efficient, but it is perfect for illustrating the protocol steps involved. This code snippet above listens for the Mission Request Sequence message sent from the Pixhawk and handles it by looking for the sequence number specified. If the Pixhawk is asking for the first way-point (sequence 0), the code will run the create_home function shown below:

The create_home() function above packs the parameters specified in the variables into a Mavlink message which specifies the home coordinates using the MAV_CMD_NAV_WAYPOINT command.

**In the mission protocol documentation, one can find other commands which can be substituted to specify DO (like activating a servo) and CONDITION commands (pausing the mission).**

Once this message is sent to the Pixhawk, the Pixhawk will then send another Mission Request Sequence message to the Arduino containing the sequence number of the way-point it expects next (sequence 1).

Once again, the Mission Request Sequence message is handled by the code snippet in step 2, and executes the create_waypoint() function

This function is pretty much identical to the create_home() function with a different GPS coordinate and seq number. I also set the guided mode to 1 just for the heck of it. After this way-point is sent and successfully received by the Pixhawk, the Pixhawk is no longer waiting for any further coordinates and sends back a MISSION_ACK message instead. This MISSION_ACK message will contain either a 0 (successful), or 1 (failure) for its TYPE.

In order to handle this message, we use the function below:

Serial output of all of the above:

Handling the MISSION_ACK message aids in deciding whether or not to retry the entire process in case something has failed (I did not show this in my code examples). This is definitely a function to incorporate into your code as I noticed uploading of way-points will sometimes fail if data-streams are being sent from the Pixhawk at the same time.

Putting it all together

If you open Mission Planner (or another ground control software) and read the way-points from the Pixhawk, you’ll see the way-point that was specified by the create_waypoint() function.