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);
  }
}

Function List

/* Send a heartbeat message to sync with ArduPilot. This needs to be done
  * at ideally 1Hz and no less than 0.2Hz.
  */
void sendHeartbeat();

/* Set initial config parameters. This needs to be done before flight. */
MAVLinkACK_t sendInitialConfigs();

/* Arm (arm=1) or disarm (arm=0) the plane. */
MAVLinkACK_t sendArmDisarm(const bool arm);

/* Change the flight mode of the plane.
  * 
  * Plane flight modes we should use:
  * - PLANE_MODE_AUTO
  * - PLANE_MODE_GUIDED
  * - PLANE_MODE_QLOITER
  * - PLANE_MODE_QLAND
  * - PLANE_MODE_QSTABILIZE
  */
MAVLinkACK_t sendFlightModeChange(const PLANE_MODE flight_mode);

/* Do a VTOL take-off. The plane must be in AUTO mode. */
MAVLinkACK_t sendVTOLTakeOff(const float altitude);

/* Add a waypoint to navigate. The plane will consider the waypoint reached when it's
  * within acceptable_range of the waypoint. The plane must be in GUIDED mode. */
MAVLinkACK_t sendWaypointNav(const float x, const float y, const float z, const float acceptable_range);

/* Clear all missions, including VTOL take-off and waypoint navigation. */
MAVLinkACK_t sendClearMissions();

/* Receive a message from ArduPilot. */
bool receiveMessage(MAVLinkMessage_t& mavlink_message);

/* Check an ACK message against an expected ACK results. */
bool checkMessageACK(const MAVLinkMessage_t mavlink_message, const MAVLinkACK_t expected_ack);

/* Check if the last navigation-type command is executed */
bool checkLastNavCmdExecuted(const MAVLinkMessage_t mavlink_message);

/* Returns the index of the last navigation-type command (UINT16_MAX if none sent) */
uint16_t getLastNavCmdIdx();

/* Constructor */
MAVLink(UART_HandleTypeDef* uart_handle);

Embedded Flight Software