MAVLink Linux/QNX/MacOs Integration Tutorial (UDP)

Overview

This program was written to test the udp connection to QGroundControl. It will send the necessary mavlink packets to QGroundControl in order to cause a new UAS object to be created and allow packets to be sent back. The code can be compiled unchanged on Linux, QNX and MacOS. Please find the correct GCC commands at the bottom of the page.

Note: In order for this code to work you must adjust the IP address of the host running qgroundcontrol. The default address is localhost (127.0.0.1), but you can change it by giving a parameter to the process, so that is the address to change.

To get full help, type after you compiled the code:

./mavlink_udp --help

You can find a full description of the details of the MAVLink protocol in the wiki.

Connection / Stateless

:!: MAVLink is stateless, but QGroundControl tracks if a system is alive using the heartbeat message. Therefore make sure to send a heartbeat every 60, 30, 10 or 1 second (1 Hz is recommended, but not required). A system will only be considered connected (and the views created for it) once a heartbeat arrives.

mavlink_udp.c
/*******************************************************************************
 Copyright (C) 2010  Bryan Godbolt godbolt ( a t ) ualberta.ca
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 ****************************************************************************/
/*
 This program sends some data to qgroundcontrol using the mavlink protocol.  The sent packets
 cause qgroundcontrol to respond with heartbeats.  Any settings or custom commands sent from
 qgroundcontrol are printed by this program along with the heartbeats.
 
 
 I compiled this program sucessfully on Ubuntu 10.04 with the following command
 
 gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c
 
 the rt library is needed for the clock_gettime on linux
 */
/* These headers are for QNX, but should all be standard on unix/linux */
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <time.h>
#if (defined __QNX__) | (defined __QNXNTO__)
/* QNX specific headers */
#include <unix.h>
#else
/* Linux / MacOS POSIX timer headers */
#include <sys/time.h>
#include <time.h>
#include <arpa/inet.h>
#endif
 
/* This assumes you have the mavlink headers on your include path
 or in the same folder as this source file */
#include <mavlink.h>
 
 
#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
 
uint64_t microsSinceEpoch();
 
int main(int argc, char* argv[])
{
 
	char help[] = "--help";
 
 
	char target_ip[100];
 
	float position[6] = {};
	int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	struct sockaddr_in gcAddr; 
	struct sockaddr_in locAddr;
	//struct sockaddr_in fromAddr;
	uint8_t buf[BUFFER_LENGTH];
	ssize_t recsize;
	socklen_t fromlen;
	int bytes_sent;
	mavlink_message_t msg;
	uint16_t len;
	int i = 0;
	//int success = 0;
	unsigned int temp = 0;
 
	// Check if --help flag was used
	if ((argc == 2) && (strcmp(argv[1], help) == 0))
    {
		printf("\n");
		printf("\tUsage:\n\n");
		printf("\t");
		printf("%s", argv[0]);
		printf(" <ip address of QGroundControl>\n");
		printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
		exit(EXIT_FAILURE);
    }
 
 
	// Change the target ip if parameter was given
	strcpy(target_ip, "127.0.0.1");
	if (argc == 2)
    {
		strcpy(target_ip, argv[1]);
    }
 
 
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;
	locAddr.sin_addr.s_addr = INADDR_ANY;
	locAddr.sin_port = htons(14551);
 
	/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
	if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
    {
		perror("error bind failed");
		close(sock);
		exit(EXIT_FAILURE);
    } 
 
	/* Attempt to make it non blocking */
	if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
    {
		fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
		close(sock);
		exit(EXIT_FAILURE);
    }
 
 
	memset(&gcAddr, 0, sizeof(gcAddr));
	gcAddr.sin_family = AF_INET;
	gcAddr.sin_addr.s_addr = inet_addr(target_ip);
	gcAddr.sin_port = htons(14550);
 
 
 
	for (;;) 
    {
 
		/*Send Heartbeat */
		mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
		/* Send Status */
		mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
 
		/* Send Local Position */
		mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), 
										position[0], position[1], position[2],
										position[3], position[4], position[5]);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
		/* Send attitude */
		mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
 
		memset(buf, 0, BUFFER_LENGTH);
		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
		if (recsize > 0)
      	{
			// Something received - print out all bytes and parse packet
			mavlink_message_t msg;
			mavlink_status_t status;
 
			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
			for (i = 0; i < recsize; ++i)
			{
				temp = buf[i];
				printf("%02x ", (unsigned char)temp);
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
				{
					// Packet received
					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
				}
			}
			printf("\n");
		}
		memset(buf, 0, BUFFER_LENGTH);
		sleep(1); // Sleep one second
    }
}
 
 
/* QNX timer version */
#if (defined __QNX__) | (defined __QNXNTO__)
uint64_t microsSinceEpoch()
{
 
	struct timespec time;
 
	uint64_t micros = 0;
 
	clock_gettime(CLOCK_REALTIME, &time);  
	micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;
 
	return micros;
}
#else
uint64_t microsSinceEpoch()
{
 
	struct timeval tv;
 
	uint64_t micros = 0;
 
	gettimeofday(&tv, NULL);  
	micros =  ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
 
	return micros;
}
#endif

Compilation

On Ubuntu Linux and Mac OS:
Adjust the relative path to your MAVLink folder, and then compile.

gcc -I ../../include/common -o mavlink_udp mavlink_udp.c

On QNX: Point the include path to the mavlink includes

gcc -I ../../include/common -lsocket -o mavlink_udp mavlink_udp.c

MAVLink Windows Integration Tutorial Using Dev C++ (TCP)

Overview

mavlink_tcp.c
/**
 * @file
 *   @brief The tcp interface process
 *
 *   This process connects any external MAVLink tcp device and prints data
 *
 *   @author Tsao, Chia-Cheng, <chiacheng.tsao@gmail.com>
 *
 */
 
#include <winsock2.h>
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <common/mavlink.h>
 
bool silent = true;              ///< Wether console output should be enabled
bool verbose = false;             ///< Enable verbose output
bool debug = false;               ///< Enable debug functions and output
 
/*
    Create a TCP socket
*/
 
int main(int argc , char *argv[])
{
    WSADATA wsa;
    SOCKET s;
    struct sockaddr_in server;
 
    printf("Initialising Winsock... \n");
 
    if (WSAStartup(MAKEWORD(2,2),&wsa) != 0)
    {
        printf("Failed. Error Code : %d \n",WSAGetLastError());
        return 1;
    }
 
    printf("Initialised.\n");
 
    //Create a socket
    if((s = socket(AF_INET , SOCK_STREAM , 0 )) == INVALID_SOCKET)
    {
        printf("Could not create socket : %d \n" , WSAGetLastError());
    }
 
    printf("Socket created.\n");
 
    server.sin_addr.s_addr = inet_addr("192.168.0.99");
    server.sin_family = AF_INET;
    server.sin_port = htons( 6789 );
 
    //Connect to remote server
    if (connect(s , (struct sockaddr *)&server , sizeof(server)) < 0)
    {
        puts("connect error! \n");
        return 1;
    }
 
    printf("Connected. \n");
 
    mavlink_status_t lastStatus;
    lastStatus.packet_rx_drop_count = 0;
 
    //Receive a reply from the server
    while(1) {
        uint8_t cp;
        mavlink_message_t message;
        mavlink_status_t status;
        uint8_t msgReceived = false;
 
        if((msgReceived = recv(s , (char*)&cp , 1 , 0)) == SOCKET_ERROR)
        {
            printf("recv failed! \n");
        }
 
        if(msgReceived > 0)
        {
            // Check if a message could be decoded, return the message in case yes
            msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
 
            if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count)
            {
                if (verbose || debug) printf("ERROR: DROPPED %d PACKETS! \n", status.packet_rx_drop_count);
                if (debug)
                {
                    unsigned char v=cp;
                    fprintf(stderr,"%02x, ", v);
                }
            }
            lastStatus = status;
        }
        else
        {
            if (!silent) fprintf(stderr, "ERROR: Could not read from tcp! \n");
        }
 
        // If a message could be decoded, handle it
        if(msgReceived)
        {
            //if (verbose || debug) std::cout << std::dec << "Received and forwarded tcp port message with id " << static_cast<unsigned int>(message.msgid) << " from system " << static_cast<int>(message.sysid) << std::endl;
 
            // Do not send images over serial port
 
            // DEBUG output
            if (debug)
            {
                fprintf(stderr,"Received tcp data: ");
                unsigned int i;
                uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
                unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
 
                if (messageLength > MAVLINK_MAX_PACKET_LEN)
                {
                    fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE \n");
                }
                else
                {
                    for (i=0; i<messageLength; i++)
                    {
                        unsigned char v=buffer[i];
                        fprintf(stderr,"%02x ", v);
                    }
                    fprintf(stderr,"\n");
                }
            }
 
            if (verbose || debug)
                printf("Received message from tcp with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
 
            /* decode and print */
 
            // For full MAVLink message documentation, look at:
            // https://pixhawk.ethz.ch/mavlink/
 
            // Only print every n-th message
            static unsigned int scaled_imu_receive_counter = 0;
 
            switch (message.msgid)
            {
                case MAVLINK_MSG_ID_HIGHRES_IMU:
                {
                    mavlink_highres_imu_t imu;
                    mavlink_msg_highres_imu_decode(&message, &imu);
 
                    printf("Got message HIGHRES_IMU");
                    printf("\t time: %llu\n", imu.time_usec);
                    printf("\t acc  (NED):\t% f\t% f\t% f (m/s^2)\n", imu.xacc, imu.yacc, imu.zacc);
                    printf("\t gyro (NED):\t% f\t% f\t% f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro);
                    printf("\t mag  (NED):\t% f\t% f\t% f (Ga)\n", imu.xmag, imu.ymag, imu.zmag);
                    printf("\t baro: \t %f (mBar)\n", imu.abs_pressure);
                    printf("\t altitude: \t %f (m)\n", imu.pressure_alt);
                    printf("\t temperature: \t %f C\n", imu.temperature);
                    printf("\n");
                }
                break;
 
                case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
                {
                    mavlink_global_position_int_t packet;
                    mavlink_msg_global_position_int_decode(&message, &packet);
                    printf("eccolo!! %d %d %d %d\n",packet.time_boot_ms,packet.lat,packet.lon,packet.hdg );
                }
                break;
 
                #if 0
                case MAVLINK_MSG_ID_ATTITUDE:
                {
                    if (scaled_imu_receive_counter % 50 == 0)
                    {
                        mavlink_attitude_t packet_in;
                        mavlink_msg_attitude_decode(&message, &packet_in);
                        printf("attitude %f %f\n",packet_in.roll,packet_in.pitch );
                    }
                    scaled_imu_receive_counter++;
                }
                break;
                #endif
            }
        }
    }
 
    return 0;
}