MAVLink UDP Example for *nix system (Linux, MacOS, BSD, etc.) and Windows

mavlink_udp.c

/*******************************************************************************
 Copyright (C) 2010  Bryan Godbolt godbolt ( a t ) ualberta.ca
 ****************************************************************************/
/*
 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 successfully on Ubuntu 10.04 with the following command
 gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c
 To compile on Windows, run:
 gcc -I ../../include/common -o udp-server udp-server-test.c -lwsock32
 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>
#ifdef WIN32
#include <winsock2.h>
#include <ws2tcpip.h>
#include <iphlpapi.h>
#include <windows.h>
#include <tchar.h>
#pragma comment(lib, "WSock32.Lib")
DWORD WINAPI ReceivingFunctionWindows(LPVOID lpParam);
DWORD WINAPI HeartbeatFunctionWindows(LPVOID lpParam);
#endif
#ifdef linux
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <stdlib.h>
#endif
#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 <stdbool.h> /* required for the definition of bool in C99 */
#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 2048
uint64_t microsSinceEpoch();
unsigned char system_id = 1;
unsigned char component_id = 1;
float position[6] = {};
int sock;
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen = sizeof(gcAddr);
int bytes_sent;
mavlink_message_t in_msg;
uint16_t len;
unsigned int temp = 0;
void heartbeatLoop()
{
    mavlink_message_t out_msg;
    for (;;)
    {
        /* Send Heartbeat */
        mavlink_msg_heartbeat_pack(system_id, component_id, &out_msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
        len = mavlink_msg_to_send_buffer(buf, &out_msg);
        bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
        /* Send Status */
        mavlink_msg_sys_status_pack(system_id, component_id, &out_msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
        len = mavlink_msg_to_send_buffer(buf, &out_msg);
        bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
        sleep(1);
    }
}
int main(int argc, char* argv[])
{
    char help[] = "--help";
    char target_ip[100];
#ifdef WIN32
    WSADATA d;
    int wsaerr;
    wsaerr = WSAStartup(MAKEWORD(2,2), &d);
    if (wsaerr != 0)
    {
        /* Tell the user that we could not find a usable */
        printf("The Winsock dll not found!\n");
        return 0;
    }
    else
    {
        printf("The Winsock dll found!\n");
        printf("The status: %s.\n", d.szSystemStatus);
    }
#endif
    // 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]);
        printf("GCS IP set to: %s\n ", target_ip);
    }
    else
    {
        printf("GCS IP set to default: %s\n", target_ip);
    }
    sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    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 */
    int result;
#if (defined WIN32)
    u_long iMode = 1;
    result = ioctlsocket(sock, FIONBIO, &iMode);
#elif (defined __QNX__) | (defined __QNXNTO__)
    result = fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC);
#else
    result = fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC);
#endif
    if (result != NO_ERROR)
    {
        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);
#if (defined WIN32)
    DWORD WINAPI HeartbeatFunctionWindows(LPVOID lpParam)
    {
        heartbeatLoop();
        return 0;
    }
    HANDLE hThread = CreateThread(NULL, 0, HeartbeatFunctionWindows, NULL, 0, NULL);
    if (hThread == NULL)
    {
        printf("Thread creation failed\n");
        return 1;
    }
    WaitForSingleObject(hThread, INFINITE);
    CloseHandle(hThread);
    WSACleanup();
#elif (defined __QNX__) | (defined __QNXNTO__)
    printf("Unix platform\n");
    void *heartbeatFunction()
    {
        heartbeatLoop();
    }
    pthread_t receiving_thread_id, heartbeat_thread_id;
    pthread_create(&heartbeat_thread_id, NULL, heartbeatFunction, NULL);
    pthread_join(heartbeat_thread_id, NULL);
#endif
}