Using the MAVLink Driver

Sending a Message

Messages can be sent to ArduPilot using functions with the send prefix. Some of them sends an instruction to ArduPilot and returns a MAVLinkACK_t struct that can be used to check ArduPilot’s ACK of the instruction.

Receiving a Message

Messages from ArduPilot can be received by calling receiveMessage. Then you can decide what to do with the message based on its type:

HEARTBEAT - a heartbeat message. This tells us that ArduPilot is still alive.
ACK - an ACK message in response to an instruction we sent. We can validate the ACK with checkMessageACK.
NAV_CMD_EXECUTED - a message notifying that a navigation command (vtol take-off, waypoint) has been executed.

Exchanging Heartbeats

ZeroPilot and ArduPilot must exchange heartbeats at 1Hz and no less than 0.2Hz to let each other know the sender is still alive.

MAVLinkMessage_t mavlink_message = {};

sendHeartbeat();

/* Do other stuff */

if (receiveMessage(mavlink_message)) {
  if (mavlink_message.type == MAVLinkMessageType::HEARTBEAT) {
    printf("Heartbeat received! (system status = %d)\n",
      mavlink_message.heartbeat.system_status);
  }
}

Sending Instructions

We can send an instruction to ArduPilot and check for a corresponding ACK message from ArduPilot.

MAVLinkMessage_t mavlink_message = {};

MAVLinkACK_t expected_ack = sendArmDisarm(true); /* Arm the plane */

if (receiveMessage(mavlink_message)) {
  if (mavlink_message.type == MAVLinkMessageType::ACK &&
      checkMessageACK(mavlink_message, expected_ack)) {
    printf("Arm instruction acknowledged!");
  }
}

Checking if a Navigation Command is Executed

We can check if navigation commands (vtol take-off, waypoint navigation) are executed by checking incoming NAV_CMD_EXECUTED messages and calling checkLastNavCmdExecuted. If we want to check a specific command out of all the commands we sent, the executed_nav_cmd_idx member in the message will tell us the exact index of the command.

MAVLinkMessage_t mavlink_message = {};

/* Take-off to 3 meters above ground */
MAVLinkACK_t expected_ack = sendVTOLTakeOff(3);

if (receiveMessage(mavlink_message)) {
  if (mavlink_message.type == MAVLinkMessageType::ACK &&
      checkMessageACK(mavlink_message, expected_ack)) {
    printf("Take-off command acknowledged!");
  }
}

if (receiveMessage(mavlink_message)) {
  if (mavlink_message.type == MAVLinkMessageType::NAV_CMD_EXECUTED &&
      checkLastNavCmdExecuted(mavlink_message)) {
    printf("Take-off command executed!");
  }
}

/* Navigate to (100,100,5) with an acceptable radius of 2 meters*/
expected_ack = sendWaypointNav(100, 100, 10, 2);

if (receiveMessage(mavlink_message)) {
  if (mavlink_message.type == MAVLinkMessageType::NAV_CMD_EXECUTED &&
      mavlink_message.executed_nav_cmd_idx == 1) {
    printf("Take-off command executed! Command index = %d",
      mavlink_message.executed_nav_cmd_idx);
  }
}

Embedded Flight Software