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

Posted on February 10, 2019May 29, 2021 by Adrian Li

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:
https://www.locarbftw.com/understanding-the-arduino-mavlink-library/

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:

GCS (client) sends MISSION_COUNT including the number of mission items to be uploaded (count)
A timeout must be started for the GCS to wait on the response from Drone (MISSION_REQUEST_INT) .
Drone (server) receives the message, and prepares to upload mission items.
Drone responds with MISSION_REQUEST_INT requesting the first mission item (seq==1).
GCS receives MISSION_REQUEST_INT and responds with the requested mission item in a MISSION_ITEM_INT message.
Drone and GCS repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle, iterating seq until all items are uploaded.
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.
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):

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.
Geofence “missions” are supported as defined in this specification.
Rally point “missions” are not supported on PX4.
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.
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

Arduino sends MISSION_COUNT including the number of mission items to be uploaded.
Pixhawk receives the message, responds with MISSION_REQUEST requesting the first mission item.
Arduino receives MISSION_REQUEST and responds with the requested mission item in a MISSION_ITEM message.
Pixhawk and Arduino repeat the MISSION_REQUEST_INT/MISSION_ITEM_INT cycle until all items are uploaded.
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).
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.

void mission_count() {
  //Step #1 of uploading a new waypoint
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point)

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count);
  //uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count

  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

void mission_count() {

//Step #1 of uploading a new waypoint

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point)

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count);

//uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

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.

     // Step 2 uploading a new waypoint - Check for mission replies
      case MAVLINK_MSG_ID_MISSION_REQUEST:
      {
       mavlink_mission_request_t missionreq;
       mavlink_msg_mission_request_decode(&msg, &missionreq);
               
        Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq);
        Serial.print("\SysID: ");Serial.println(missionreq.target_system);
        Serial.print("\CompID: ");Serial.println(missionreq.target_component);

        if (missionreq.seq == 0) {
        create_home();
        Serial.print("Sent Home: \n");
        }

        if (missionreq.seq == 1) {
        create_waypoint();
        Serial.print("Sent Waypoint: \n");
        }
       
      }
      break;

// Step 2 uploading a new waypoint - Check for mission replies

case MAVLINK_MSG_ID_MISSION_REQUEST:

{

mavlink_mission_request_t missionreq;

mavlink_msg_mission_request_decode(&msg, &missionreq);

Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq);

Serial.print("\SysID: ");Serial.println(missionreq.target_system);

Serial.print("\CompID: ");Serial.println(missionreq.target_component);

if (missionreq.seq == 0) {

create_home();

Serial.print("Sent Home: \n");

}

if (missionreq.seq == 1) {

create_waypoint();

Serial.print("Sent Waypoint: \n");

}

break;

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:

void create_home() {
  //Step 3 of uploading a new waypoint (send HOME coordinates)
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t seq = 0; // Sequence number
  uint8_t frame = 0; // Set target frame to global default
  uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types
  uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item.
  uint8_t autocontinue = 0; // Always 0
  float param1 = 0; // Loiter time
  float param2 = 0; // Acceptable range from target - radius in meters
  float param3 = 0; // Pass through waypoint
  float param4 = 0; // Desired yaw angle
  float x = 45.464217; // Latitude - degrees
  float y = -1.280222; // Longitude - degrees
  float z = 200; // Altitude - meters

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  
  //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z
  
  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

void create_home() {

//Step 3 of uploading a new waypoint (send HOME coordinates)

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t seq = 0; // Sequence number

uint8_t frame = 0; // Set target frame to global default

uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types

uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item.

uint8_t autocontinue = 0; // Always 0

float param1 = 0; // Loiter time

float param2 = 0; // Acceptable range from target - radius in meters

float param3 = 0; // Pass through waypoint

float param4 = 0; // Desired yaw angle

float x = 45.464217; // Latitude - degrees

float y = -1.280222; // Longitude - degrees

float z = 200; // Altitude - meters

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);

//uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

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

void create_waypoint() {
  //Step 3 continuation of uploading a new waypoint (send 1st coordinates)
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t seq = 1; // Sequence number
  uint8_t frame = 0; // Set target frame to global default
  uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types
  uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item.
  uint8_t autocontinue = 0; // Always 0
  float param1 = 0; // Loiter time
  float param2 = 0; // Acceptable range from target - radius in meters
  float param3 = 0; // Pass through waypoint
  float param4 = 0; // Desired yaw angle
  float x = 15.464217; // Latitude - degrees
  float y = -11.280222; // Longitude - degrees
  float z = 200; // Altitude - meters

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  
  //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z
  
  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

void create_waypoint() {

//Step 3 continuation of uploading a new waypoint (send 1st coordinates)

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t seq = 1; // Sequence number

uint8_t frame = 0; // Set target frame to global default

uint16_t command = MAV_CMD_NAV_WAYPOINT; // Can be substituted with other commands https://mavlink.io/en/services/mission.html#mission_types

uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item.

uint8_t autocontinue = 0; // Always 0

float param1 = 0; // Loiter time

float param2 = 0; // Acceptable range from target - radius in meters

float param3 = 0; // Pass through waypoint

float param4 = 0; // Desired yaw angle

float x = 15.464217; // Latitude - degrees

float y = -11.280222; // Longitude - degrees

float z = 200; // Altitude - meters

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

This function is pretty much identical to the create_home() function with a different GPS coordinate and seq number. I also set “current” to 1 to specify it as the current mission item (does it really have an effect if the mission is on auto?). 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:

case MAVLINK_MSG_ID_MISSION_ACK:
      {
       mavlink_mission_ack_t missionack;
       mavlink_msg_mission_ack_decode(&msg, &missionack);
        
        Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type);
        Serial.print("\SysID: ");Serial.println(missionack.target_system);
        Serial.print("\CompID: ");Serial.println(missionack.target_component);

        if (missionack.type == 1) {
        Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type);
        }

        if (missionack.type == 0) {
        Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type);
        }
    }
      break;

case MAVLINK_MSG_ID_MISSION_ACK:

{

mavlink_mission_ack_t missionack;

mavlink_msg_mission_ack_decode(&msg, &missionack);

Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type);

Serial.print("\SysID: ");Serial.println(missionack.target_system);

Serial.print("\CompID: ");Serial.println(missionack.target_component);

if (missionack.type == 1) {

Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type);

}

if (missionack.type == 0) {

Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type);

}

break;

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

#include <mavlink.h>
//#include <SoftwareSerial.h>

//#define RXpin 0
//#define TXpin 1
//SoftwareSerial SerialMAV(RXpin, TXpin); // sets up serial communication on pins 3 and 2

void setup() {
  Serial1.begin(57600); //RXTX from Pixhawk (Port 19,18 Arduino Mega)
  Serial.begin(57600); //Main serial port to read the port

  mission_count();
  
}

void loop() {

 MavLink_receive();

}

//function called by arduino to read any MAVlink messages sent by serial communication from flight controller to arduino
void MavLink_receive()
  { 
  mavlink_message_t msg;
  mavlink_status_t status;

  while(Serial1.available())
  {
    uint8_t c= Serial1.read();

    //Get new message
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
    {

    //Handle new message from autopilot
      switch(msg.msgid)
      {
      
      // Step 2 uploading a new waypoint - Check for mission replies
      case MAVLINK_MSG_ID_MISSION_REQUEST:
      {
       mavlink_mission_request_t missionreq;
       mavlink_msg_mission_request_decode(&msg, &missionreq);
               
        Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq);
        Serial.print("\SysID: ");Serial.println(missionreq.target_system);
        Serial.print("\Compid: ");Serial.println(missionreq.target_component);

        if (missionreq.seq == 0) {
        create_home();
        Serial.print("Sent Home: \n");
        }

        if (missionreq.seq == 1) {
        create_waypoint();
        Serial.print("Sent Waypoint: \n");
        }
      }
      break;

      case MAVLINK_MSG_ID_MISSION_ACK:
      // Step 4 uploading a new waypoint - Receive Mission Ack Message
      {
       mavlink_mission_ack_t missionack;
       mavlink_msg_mission_ack_decode(&msg, &missionack);
       
        Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type);
        Serial.print("\SysID: ");Serial.println(missionack.target_system);
        Serial.print("\CompID: ");Serial.println(missionack.target_component);

        if (missionack.type == 1) {
        Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type);
        }

        if (missionack.type == 0) {
        Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type);
        }   
      }
      break;
      
      }
    }
  }
}

void mission_count() {
  //Step #1 of uploading a new waypoint
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point)

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count);
  //uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count

  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

void create_home() {
  //Step 3 of uploading a new waypoint (send HOME coordinates)
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t seq = 0; // Sequence number
  uint8_t frame = 0; // Set target frame to global default
  uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4
  uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item.
  uint8_t autocontinue = 0; // Always 0
  float param1 = 0; // Loiter time
  float param2 = 0; // Acceptable range from target - radius in meters
  float param3 = 0; // Pass through waypoint
  float param4 = 0; // Desired yaw angle
  float x = 45.464217; // Latitude - degrees
  float y = -1.280222; // Longitude - degrees
  float z = 200; // Altitude - meters

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z
  
  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

void create_waypoint() {
  //Step 3 continuation of uploading a new waypoint (send 1st coordinates)
  uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
  uint8_t _component_id = 2; // component id of sending station 2 works fine
  uint8_t _target_system = 1; // Pixhawk id
  uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

  uint16_t seq = 1; // Sequence number
  uint8_t frame = 0; // Set target frame to global default
  uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4
  uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item.
  uint8_t autocontinue = 0; // Always 0
  float param1 = 0; // Loiter time
  float param2 = 0; // Acceptable range from target - radius in meters
  float param3 = 0; // Pass through waypoint
  float param4 = 0; // Desired yaw angle
  float x = 31.0529340; // Latitude - degrees
  float y = -162.0703125; // Longitude - degrees
  float z = 200; // Altitude - meters

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  //uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z
  
  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  
  // Send the message (.write sends as bytes)
  Serial1.write(buf, len);
  
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

#include <mavlink.h>

//#include <SoftwareSerial.h>

//#define RXpin 0

//#define TXpin 1

//SoftwareSerial SerialMAV(RXpin, TXpin); // sets up serial communication on pins 3 and 2

void setup() {

Serial1.begin(57600); //RXTX from Pixhawk (Port 19,18 Arduino Mega)

Serial.begin(57600); //Main serial port to read the port

mission_count();

}

void loop() {

MavLink_receive();

}

//function called by arduino to read any MAVlink messages sent by serial communication from flight controller to arduino

void MavLink_receive()

{

mavlink_message_t msg;

mavlink_status_t status;

while(Serial1.available())

{

uint8_t c= Serial1.read();

//Get new message

if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))

{

//Handle new message from autopilot

switch(msg.msgid)

{

// Step 2 uploading a new waypoint - Check for mission replies

case MAVLINK_MSG_ID_MISSION_REQUEST:

{

mavlink_mission_request_t missionreq;

mavlink_msg_mission_request_decode(&msg, &missionreq);

Serial.print("\nMission Req Sequence: ");Serial.println(missionreq.seq);

Serial.print("\SysID: ");Serial.println(missionreq.target_system);

Serial.print("\Compid: ");Serial.println(missionreq.target_component);

if (missionreq.seq == 0) {

create_home();

Serial.print("Sent Home: \n");

}

if (missionreq.seq == 1) {

create_waypoint();

Serial.print("Sent Waypoint: \n");

}

break;

case MAVLINK_MSG_ID_MISSION_ACK:

// Step 4 uploading a new waypoint - Receive Mission Ack Message

{

mavlink_mission_ack_t missionack;

mavlink_msg_mission_ack_decode(&msg, &missionack);

Serial.print("\nMission Ack Sequence: ");Serial.println(missionack.type);

Serial.print("\SysID: ");Serial.println(missionack.target_system);

Serial.print("\CompID: ");Serial.println(missionack.target_component);

if (missionack.type == 1) {

Serial.print("\nMission upload FAILED: ");Serial.println(missionack.type);

}

if (missionack.type == 0) {

Serial.print("\nMission upload SUCCESSFULL: ");Serial.println(missionack.type);

}

break;

}

void mission_count() {

//Step #1 of uploading a new waypoint

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t count = 2; // How many items to upload (HOME coordinates are always the first way-point)

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_count_pack(_system_id, _component_id, &msg, _target_system, _target_component, count);

//uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

void create_home() {

//Step 3 of uploading a new waypoint (send HOME coordinates)

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t seq = 0; // Sequence number

uint8_t frame = 0; // Set target frame to global default

uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4

uint8_t current = 0; // false:0, true:1 - When downloading, whether the item is the current mission item.

uint8_t autocontinue = 0; // Always 0

float param1 = 0; // Loiter time

float param2 = 0; // Acceptable range from target - radius in meters

float param3 = 0; // Pass through waypoint

float param4 = 0; // Desired yaw angle

float x = 45.464217; // Latitude - degrees

float y = -1.280222; // Longitude - degrees

float z = 200; // Altitude - meters

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

void create_waypoint() {

//Step 3 continuation of uploading a new waypoint (send 1st coordinates)

uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software

uint8_t _component_id = 2; // component id of sending station 2 works fine

uint8_t _target_system = 1; // Pixhawk id

uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)

uint16_t seq = 1; // Sequence number

uint8_t frame = 0; // Set target frame to global default

uint16_t command = MAV_CMD_NAV_WAYPOINT; // Specific command for PX4

uint8_t current = 1; // false:0, true:1 - When downloading, whether the item is the current mission item.

uint8_t autocontinue = 0; // Always 0

float param1 = 0; // Loiter time

float param2 = 0; // Acceptable range from target - radius in meters

float param3 = 0; // Pass through waypoint

float param4 = 0; // Desired yaw angle

float x = 31.0529340; // Latitude - degrees

float y = -162.0703125; // Longitude - degrees

float z = 200; // Altitude - meters

// Initialize the required buffers

mavlink_message_t msg;

uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message

mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);

// Copy the message to the send buffer

uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message (.write sends as bytes)

Serial1.write(buf, len);

}

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.

41 thoughts on “Uploading a waypoint and interfacing the Pixhawk to an Arduino using Mavlink”

Ashwani says:

April 5, 2020 at 11:32 pm

Awesome work!!!

Reply
Ashwani says:

April 5, 2020 at 11:33 pm

Thank you so much! You are genius. Hope you are safe in this corona time.

Regards

Reply
Ishan says:

August 28, 2020 at 11:37 pm

How we can upload waypoint into arduino

Reply
Alex says:

September 3, 2020 at 4:32 am

Thanks very much.
I upload code on my board and program change way point correctly but home point doesn’t change though I receive “Sent Home:” message on monitor serial.
Any idea for my problem?

Reply
1. Adrian Li says:
  
  September 3, 2020 at 8:34 am
  
  My understanding is that it doesn’t really matter. As soon as you arm the autopilot home coordinates are set automatically.
  
  Reply
  1. Alex says:
    
    September 5, 2020 at 1:49 am
    
    Thanks for answer.
    A question,Do I read previous waypoint before that write new way point with arduino? I don’t find any command for this.
    
    Reply
    1. Adrian Li says:
      
      September 5, 2020 at 8:25 am
      
      Uploading a new mission plan will overwrite any existing mission plan.
      
      Reply
honghai says:

January 5, 2021 at 9:33 am

Get perfect results in ESP32 verification. TKS!

Can you kindly try to introduce: How to Use ESP32 to coding mavproxy

Reply
gizem karabiber says:

September 20, 2021 at 3:50 am

Hi,
I am trying to new create a waypoint.Althogh I run your code,there is no difference on plan map.
Do you have any suggestions?

Reply
1. Adrian Li says:
  
  September 20, 2021 at 7:21 am
  
  Make sure you turn off all other messaging prior to running the code. Also check to make sure communication between pixhawk and Arduino is working first
  
  Reply
  1. gizem karabiber says:
    
    September 21, 2021 at 6:45 am
    
    16:41:41.881 -> Mission Req Sequence: 0
    16:41:41.881 -> SysID: 255
    16:41:41.881 -> Compid: 2
    16:41:41.881 -> Sent Home:
    16:41:41.928 ->
    16:41:41.928 -> Mission Req Sequence: 1
    16:41:41.928 -> SysID: 255
    16:41:41.928 -> Compid: 2
    16:41:41.928 -> Sent Waypoint:
    16:41:41.928 ->
    16:41:41.928 -> Mission Ack Sequence: 0
    16:41:41.928 -> SysID: 255
    16:41:41.928 -> CompID: 2
    16:41:41.975 ->
    16:41:41.975 -> Mission upload SUCCESSFULL: 0
    I get such a response on the com port.
    But there is no change on the mission planner map screen. Do I need to do something different for this?
    
    Reply
    1. Adrian Li says:
      
      September 21, 2021 at 7:18 am
      
      After the mission is uploaded connect mission planner to the pixhawk and under the flight plan section click read wps. It will read the info from the pixhawk and should show the updated waypoint sent via Arduino.
      
      Reply
      1. gizem karabiber says:
        
        September 22, 2021 at 12:42 am
        
        Yes, thank you very much for your answer.It works.I have one more question.For example if a new waypoint comes from arduino to pixhawk , Will the plane go in that direction?
        I mean that when a new point comes from arduino, as you said I should read the mission on mission planner to be able to see.
        Does the plane need anything like writing or reading to move in that direction with the new incoming command?or just is it enough for the plane to go on the new route?
      2. Adrian Li says:
        
        September 22, 2021 at 7:24 am
        
        After a new update you’ll need to reboot the pixhawk as I found it would sometimes not begin the new plan. If you reboot the pixhawk I also found you need to reset the Arduino after the pixhawk reboots in order to regain serial communication. If you have the pixhawk configured for full autonomous operation like listed on my pixhawk settings page, the autopilot should begin working with the new flight plan.
gizem karabiber says:

September 24, 2021 at 6:18 am

I guess you are talking about this page https://www.locarbftw.com/ardurover-pixhawk-settings-for-autonomous-operation/#
Also, thank you very much for your explanatory and instructive messages.

Reply
1. Adrian Li says:
  
  September 24, 2021 at 7:16 am
  
  Yep that’s the one, No problem!
  
  Reply
ramin says:

October 22, 2021 at 2:18 pm

hi
firstly thanks for your nice tutorial
how i can read(download) mission with arduino to be sure about process

Reply
1. Adrian Li says:
  
  October 23, 2021 at 11:56 am
  
  The example sketch already contains the answer as “missionack.”
  
  The pixhawk will return a 0 (success) or 1 (failure) depending on the new mission plan upload. Just write a function to reupload the mission if the pixhawk returns a value of “1.”
  
  Reply
  1. ramin says:
    
    October 23, 2021 at 8:22 pm
    
    thanks
    
    Reply
Alex says:

November 7, 2021 at 2:23 pm

Thanks a lot for the informative post. Could you make a tutorial on how to download the current mission list from the pixhawk to the arduino/rpi board? Thanks!

Reply
Vijay says:

June 21, 2022 at 7:38 pm

I found your tutorial very useful. My objective was to get the rc in and servo out data. This worked well. But msgid 35 only gives 8 channels. Msgid 65 gets 16 channels. But how do I call 65 ?

Reply
Hemanth says:

December 16, 2022 at 2:55 pm

Hey, thanks for the great tutorial. I’m using UAV to measure soil moisture. So, when the UAV lands, soil moisture will be measured at that location. I’m using automatic flight plans with mission planner to navigate between points and land. So, once the payload measures soil moisture, I need it to send a message to pixhawk to go to next location. I’m using arduino mega. Please let me know, what command to use to achieve this.

Reply
1. Adrian Li says:
  
  December 16, 2022 at 6:15 pm
  
  One solution might be to set the UAV to land at each waypoint then have the Arduino send a set mode (auto) command after each soil sample is taken.
  
  https://www.locarbftw.com/arduino-mavlink-library-changing-flight-modes/
  
  Reply
  1. Hemanth says:
    
    March 28, 2023 at 1:51 pm
    
    Hey Adrian,
    
    Thanks a lot for the reply and sorry for such a late response. I will implement this and keep you posted. You are a saviour man.
    
    Reply
  2. Hemanth says:
    
    March 29, 2023 at 9:21 am
    
    Hello,
    I have a small doubt, once the UAV lands, it will disarm the rotors right, so if I set to automode, will automatically arm the rotor. Or should I arm the rotor through my program please let me know.
    
    Reply
    1. Adrian Li says:
      
      March 29, 2023 at 10:00 am
      
      Once you have the drone land, you should be able to send a mode command through your program to change the mode on the pixhawk. This should allow it to continue on it’s mission. Unfortunately I’m not sure how all that works together in your intended usage so it’s just trial and error really…
      
      Reply
      1. Hemanth says:
        
        June 30, 2023 at 2:13 pm
        
        I got it to work. I created an auto mission in the mission planner. Once I land, there is a delay for the payload to perform its work. Once completed, the Arduino sends a take-off command and flies to the following location. The only issue is sometimes it arms the drone and doesn’t take off. I’m wondering if this is because the delay is still in action or if the waypoints are too close to each other. Your suggestion will be helpful.
      2. Hemanth says:
        
        July 6, 2023 at 10:37 am
        
        Hello Adrian,
        I’m trying to implement similar workflow for my application. I want to upload five waypoints, home, arm, take off from home, land, dis arm. Once I upload it, I will change it to auto mode. However, when my code is trying to upload waypoints, I get the error Failed to update home(1), it is arming and disarming, and this sequence continues and I keep getting the same error. Any suggestion will be helpful. Please let me know if you need more information.
      3. Adrian Li says:
        
        July 6, 2023 at 10:45 am
        
        I actually believe home is set when the autopilot is turned on… So there is no reason to update home. Just begin with uploading waypoints
Hemanth says:

June 30, 2023 at 2:14 pm

I got it to work. I created an auto mission in the mission planner. Once I land, there is a delay for the payload to perform its work. Once completed, the Arduino sends a take-off command and flies to the following location. The only issue is sometimes it arms the drone and doesn’t take off. I’m wondering if this is because the delay is still in action or if the waypoints are too close to each other. Your suggestion will be helpful.

Reply
1. Adrian Li says:
  
  July 6, 2023 at 10:45 am
  
  You may need to send a command to auto mode versus arm only
  
  Reply
Hemanth says:

July 6, 2023 at 10:51 am

But when I arm, instead of taking off, it throughs me an error Failed to upload home(1)

Reply
1. Adrian Li says:
  
  July 6, 2023 at 10:55 am
  
  Unsure. It may be something with the pixhawk configuration or mission plan you’re uploading.
  
  Reply
  1. Hemanth says:
    
    July 7, 2023 at 3:04 pm
    
    I’m following a similar work flow to the program here.
    missionreq.seq==0 is arm
    missionreq.seq==1 is takeoff
    missionreq.se==2 is land
    
    however in my case it is never going to take off and land, it is always entering arm. The UAV is arming again and again instead of taking off
    
    Reply
    1. Adrian Li says:
      
      July 8, 2023 at 1:31 pm
      
      Best bet is create a sketch just doing those mission commands with nothing else. You’ll be able to narrow down at what part the mission is failing. Is the autopilot giving you confirmations of successful mission upload?
      
      Reply
      1. Hemanth says:
        
        July 14, 2023 at 1:33 pm
        
        I’m able to upload the mission and perform the takeoff and landing. The issue was having the arm command as the first waypoint, it should not be part of the mission. I’m having a new problem now, the takeoff height specified in the program was 10m, but UAV takes off to only 1m and moves to the landing location in this height and it landed badly and damaged my propeller. Any suggestion on how to overcome this issue?
      2. Adrian Li says:
        
        July 14, 2023 at 2:50 pm
        
        You may want to play around with these settings that deal with altitude…such as verify height. You might want to see if there is a “waypoint radius” setting which applies to altitude as well
        
        https://ardupilot.org/copter/docs/common-planning-a-mission-with-waypoints-and-events.html
hemanth says:

July 6, 2023 at 10:58 am

Will it be helpful If i share my code? Please let me know.

Reply
Hemanth says:

July 25, 2023 at 1:32 pm

Hello Adrian,

Thanks for the support. I solved all my issues and everything is working as per requirement. The UAV was not taking off to the desired height because of the FRAME selection. I selected frame =5, which means it uses absolute height. Once I changed it to 6, It uses relative height and takes off to 10m every time.

additionally, the first waypoint is home, with sequence =0, and the frame has to be 0 for some reason, If I choose the frame to be 5 or 6, it does not go to the next waypoint sequence, instead keeps coming back to home waypoint upload again and again.

Reply
1. Adrian Li says:
  
  July 25, 2023 at 3:10 pm
  
  Glad to hear it’s all working now!
  
  Reply
Yuna says:

July 26, 2023 at 8:16 pm

Can you please tell me how to send coordinate, ground speed, times, iTOW, etc to mission plannner SEPARATELY? I wanna start from simple manipulation data which is generated from arduino not yours from complex project i should breakdown one by one.

Reply