Application of MAVLink and PX4 commands towards autonomous takeoff and landing.
Table of Contents
The scope of this document covers the sample source code for takeoff and landing in simulation with software-in-the-loop (SITL) for a quadcopter under autonomous control via the Intel Aero compute board.
The sample, takeoff.cpp, uses MAVLink and PX4 commands and messages to affect the takeoff and landing of the quadcopter under the control of a C++ application.
For further information on the setup and usage of the development environment for simulation with SITL, please consult the document “Set Up of Aero Compute Board Simulation Development Environment”.
The MAVLink library is a header-only library wherein the headers are generated programmatically via Python* scripts that parse XML files. The XML files fully describe the messages and commands that comprise the library. Information on securing the libraries via the GitHub repository can be found at https://github.com/mavlink/mavlink. For a comprehensive overview of MAVLink, please visit the MAVLink website.
For the purpose of this document and use case, the PX4 headers are required. The entire PX4 library can be found at https://github.com/PX4/Firmware. For a comprehensive overview of PX4, please see the PX4 User Guide and the PX4 Developer Guide.
The takeoff.cpp application is comprised of three distinct logical steps in its execution. They are as follows:
- Ensure connectivity to the drone: For a real drone this may be via a serial connection through UART; or in the case of simulation, a UDP connection to the simulation environment.
- Receipt of data (inside a never-ending loop): The application receives “heartbeat” messages from the flight controller (necessary for either real drone or simulator) to ensure that drone is in operation; or messages from a ground control station (GCS).
- Sending of data (inside the above-mentioned loop): the code sends application data from the Aero compute board (for a real drone) or a quadcopter (under simulation) to the PX4 flight controller. For purposes of providing autonomy to the drone, this step in the application logic would be sending the MAVLink and PX4 messages and commands, plus accompanying data, to the PX4 flight controller. This step may depend on whatever is computed in the Intel Aero compute board for execution at any time during the flight.
This function does the setup and connectivity to the drone via a UDP connection. It establishes an input link on port 14550 (this can change depending on your simulation environment); and an output link on port 14556 (again, this too can change depending on your simulation environment.) Next, the function attempts to open a socket connection and, in sequence, checks whether the socket failed to open; whether the socket can be set as reusable; whether binding to the socket is possible; and finally, setting the socket to a non-blocking state. Failure on any one of these checks is considered an error condition, which closes the socket and terminates the program.
This function reads and parses available incoming data, and when there is available data it handles the incoming data stream. The MAVLink project provides a set of helper functions to deal with MAVLink messages. In receiving data, mavlink_parse_char is used to parse MAVLink messages on a byte-by-byte basis. The prime use case for parsing the incoming data is to watch for “heartbeat” messages. Handling this and other messages coming from the outside is performed by the handle_message() function, which upon review is not much more than a switch-case statement handling the MAVLINK_MSG_ID_HEARTBEAT message.
The function send_data() is responsible for organizing and packaging the messages, commands, and data that are to be sent the PX4 flight controller. The messages and commands may be MAVLink or PX4 messages, or a mix of both.
In the takeoff and land example, the first parts of send_data() are dedicated to declaring the variables and MAVLink message structure that will be used later in the function. Next, the MAVLink set-mode specifies that the autopilot-specific mode is ready. It also indicates that the base-mode MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED that specifies that the drone is “armed” and that the custom mode is ready. Next, it checks to ensure that a “heartbeat” has been detected; and if not, it returns out of the function.
A two-state machine is implemented, which cycles between two modes:
- Landing – This is achieved by setting the custom mode to PX4_CUSTOM_MAIN_MODE_AUTO & PX4_CUSTOM_SUB_MODE_AUTO_LAND.
- Takeoff – This is produced by setting the custom mode to PX4_CUSTOM_MAIN_MODE_AUTO & PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF.
When this bit of state logic is done, the function continues on to encode the MAVLink message using additional MAVLink helper functions – in this case, mavlink_msg_set_mode_encode. Later, it sends the message to a buffer using mavlink_msg_to_send_buffer. Once the message is encoded and packed in the buffer, the MAVLink message is ready to be sent through the existing UDP connection. More logic is introduced to check whether the message was successfully sent or not.
This document presents a minimalistic application that demonstrates a basic takeoff and landing. It demonstrates bidirectional communication via a UDP connection – either a host in a simulation or an actual Intel Aero compute board – and a PX4 flight controller. The main concern for autonomous flight control lies in the ability of the application to encode and send messages, commands, and data to the flight controller using MAVLink or PX4 commands, or a mix of both. This is mostly the domain of the send_data() function. Additional enhancements could include introducing a fuller state-machine and decoupling the aspect of sending data from the flight-mode state.