pixhawk通过udp接收mavlink消息的简单例程

开发这个简单例程的初衷是想把在github上能在linux系统跑的通过udp接收mavlink消息的程序移植到windows系统下。

原始文件的下载和简单修改

github源代码地址：https://github.com/proto3/MAVkit，按照它的readme文件可以实现通过串口，udp，tcp，log文件等方式接收或者发送mavlink消息。

可能它给的例程不太直观，我这里把它的源码改动了一部分，通过控制台把实时的状态打印出来。由于最终的目的是要把UDP的这部分移植到windows下，所以，我改动的地方是MAVkit\src\mavkit\MavlinkUDP.cpp文件，在其中解析出一帧mavlink消息的条件中加一行

printf ("received message with id %d,sequence %d,from component %d of system %d!\n",msg.msgid,msg.seq,msg.compid,msg.sysid);

即把原来：

if(mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status))
{
    std::vector::iterator it = listeners.begin();
    for(;it != listeners.end();++it)
    {
        (*it)->send_message(msg);
    }
}

改成：

if(mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status))
{
    printf ("received message with id %d,sequence %d,from component %d of system %d!\n",msg.msgid,msg.seq,msg.compid,msg.sysid);
    std::vector::iterator it = listeners.begin();
    for(;it != listeners.end();++it)
    {
        (*it)->send_message(msg);
    }
}

改完后编译工程，在build目录下执行make命令

make

，然后在命令行下运行程序：

./mavkit --udp_server 14550

如果是用串口，仍然可以用这一方法，修改MAVkit\src\mavkit\MavlinkSerial.cpp文件，在编译完后运行

./mavkit --tty /dev/ttyACM0 57600

它还可以发送mavlink消息，按照它的格式，在main函数中发送，这里我改动地方比较多，所以提供全部的代码，整个mavkit.cpp，如果有兴趣可以和原文件进行对比。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION     0b0000110111111000
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY     0b0000110111000111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION 0b0000110000111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_FORCE        0b0000111000111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE    0b0000100111111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE     0b0000010111111111

struct Mavlink_Messages 
{

    int sysid;
    int compid;
    mavlink_heartbeat_t heartbeat;
    mavlink_attitude_t attitude;
    mavlink_att_pos_mocap_t att_pos_mocap;
    mavlink_set_position_target_local_ned_t set_position_target_local_ned;
    mavlink_position_target_local_ned_t position_target_local_ned;
};


void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &sp)
{
    sp.type_mask =
        MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION;
    sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
    sp.x = x;
    sp.y = y;
    sp.z = z;
    //printf("POSITION SETPOINT XYZ = [ %.4f , %.4f , %.4f ] \n", sp.x, sp.y, sp.z);
}


void set_yaw(float yaw, mavlink_set_position_target_local_ned_t &sp)
{
    sp.type_mask &=
        MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE;
    sp.yaw = yaw;
    //printf("POSITION SETPOINT YAW = %.4f \n", sp.yaw);
}


Mavlink_Messages current_messages;

MavMessengerInterface* master = NULL;
std::vector outputs;

uint32_t count = 0;

//----------------------------------------------------------------------------//
void usage()
{
    std::cout << "usage: mavkit master output1 output2 ..." << std::endl;
    std::cout << "  master/output: IP address, tty path or log file." << std::endl;
}
//----------------------------------------------------------------------------//
void add_messenger(MavMessengerInterface* messenger)
{
    if(master == NULL)
    {
        master = messenger;
    }
    else
    {
        outputs.push_back(messenger);
        master->append_listener(messenger);
        messenger->append_listener(master);
    }
}
//----------------------------------------------------------------------------//
bool isdigit(char *str)
{
    char c = str[0];
    int i = 1;
    while(c != '\0')
    {
        if(!isdigit(c))
        {
            return false;
        }
        c = str[i];
        i++;
    }
    return i > 1;
}
//----------------------------------------------------------------------------//
int main(int argc, char* argv[])
{
    int c;
    while (true)
    {
        static struct option long_options[] =
        {
            /* These options set a flag. */
            //   {"verbose", no_argument,       &verbose_flag, 1},
            //   {"brief",   no_argument,       &verbose_flag, 0},
            /* These options don’t set a flag.
            We distinguish them by their indices. */
            {"tty",        required_argument, 0, 'a'},
            {"udp_client", required_argument, 0, 'b'},
            {"udp_server", required_argument, 0, 'c'},
            {"tcp_client", required_argument, 0, 'd'},
            {"tcp_server", required_argument, 0, 'e'},
            {"file",       required_argument, 0, 'f'},
            {"log",        no_argument,       0, 'g'},
            {"display",    no_argument,       0, 'h'},
            {0, 0, 0, 0}
        };

        /* getopt_long stores the option index here. */
        int option_index = 0;
        c = getopt_long (argc, argv, "abcdef", long_options, &option_index);

        /* Detect the end of the options. */
        if (c == -1)
            break;

        switch (c)
        {
            // case 0:
            // {
            //     /* If this option set a flag, do nothing else now. */
            //     if (long_options[option_index].flag != 0)
            //     break;
            //     printf ("option %s", long_options[option_index].name);
            //     if (optarg)
            //     printf (" with arg %s", optarg);
            //     printf ("\n");
            //     break;
            // }
            case 'a':
            {
                if(optind + 1 > argc)
                {
                    std::cout << "./mavkit: option \'--tty\' requires two arguments" << std::endl;
                    exit(0);
                }

                char* device   = argv[--optind];
                char* baudrate_str = argv[++optind];
                optind++;
                if(isdigit(baudrate_str))
                {
                    add_messenger(new MavlinkSerial(device, atoi(baudrate_str)));
                }
                else
                {
                    //TODO baudrate not a number
                    usage();
                    exit(0);
                }
                break;
            }
            case 'b':
            {
                if(optind + 1 > argc)
                {
                    std::cout << "./mavkit: option \'--udp_client\' requires two arguments" << std::endl;
                    exit(0);
                }

                char* ip       = argv[--optind];
                char* port_str = argv[++optind];
                optind++;

                if(isdigit(port_str))
                {
                    add_messenger(new MavlinkUDP(ip, atoi(port_str)));
                }
                else
                {
                    std::cout << "./mavkit: option \'--udp_client\' port is not a number" << std::endl;
                    exit(0);
                }
                break;
            }
            case 'c':
            {
                if(isdigit(optarg))
                {
                    add_messenger(new MavlinkUDP(atoi(optarg)));
                }
                else
                {
                    std::cout << "./mavkit: option \'--udp_server\' port is not a number" << std::endl;
                    exit(0);
                }
                break;
            }
            case 'd':
            {
                if(optind + 1 > argc)
                {
                    std::cout << "./mavkit: option \'--udp_client\' requires two arguments" << std::endl;
                    exit(0);
                }

                char* ip       = argv[--optind];
                char* port_str = argv[++optind];
                optind++;

                if(isdigit(port_str))
                {
                    add_messenger(new MavlinkTCP(ip, atoi(port_str)));
                }
                else
                {
                    std::cout << "./mavkit: option \'--udp_client\' port is not a number" << std::endl;
                    exit(0);
                }
                break;
            }
            case 'e':
            {
                if(isdigit(optarg))
                {
                    add_messenger(new MavlinkTCP(atoi(optarg)));
                }
                else
                {
                    std::cout << "./mavkit: option \'--udp_server\' port is not a number" << std::endl;
                    exit(0);
                }
                break;
            }
            case 'f':
            {
                add_messenger(new MavlinkLogReader(optarg, 1.0, 0)); // speed_multiplier and start time
                break;
            }
            case 'g':
            {
                add_messenger(new MavlinkLogWriter("log/"));
                break;
            }
            case 'h':
            {
                add_messenger(new MavlinkDisplay());
                break;
            }
            default:
              exit(0);
        }
    }

      /* Instead of reporting ‘--verbose’
         and ‘--brief’ as they are encountered,
         we report the final status resulting from them. */
    //   if (verbose_flag)
    //     puts ("verbose flag is set");

    /* Print any remaining command line arguments (not options). */
    if (optind < argc)
    {
        printf ("non-option ARGV-elements: ");
        while (optind < argc)
        printf ("%s ", argv[optind++]);
        putchar ('\n');
    }

    if(master == NULL)
    {
        std::cerr << "Need at least a master messenger interface." << std::endl;
        exit(0);
    }

    master->start();
    for(int i=0;istart();

    //send Mavlink2 heartbeat to force protocol version
    while(true)
    {
    count++;
        //MSG1
    int system_id = 1;    
    int component_id = 1; 
    //必须是1,1才能发送成功目标点
        mavlink_message_t msg;

    //=========================================================================//
    /*
        mavlink_msg_heartbeat_pack(system_id, component_id, &msg, MAV_TYPE_SUBMARINE, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY);
        master->send_message(msg);
        usleep(40000);
    */
    //=========================================================================//

    set_position(0.5, 2.5, -1.5, current_messages.set_position_target_local_ned);
    set_yaw(0, current_messages.set_position_target_local_ned);
    mavlink_set_position_target_local_ned_t sp = current_messages.set_position_target_local_ned;
    sp.time_boot_ms = (uint32_t)0;
    sp.target_system = system_id;
    sp.target_component = component_id;

    //mavlink_message_t msg;
    mavlink_msg_set_position_target_local_ned_encode(system_id, component_id, &msg, &sp);
    master->send_message(msg);
    usleep(40000);

    //=========================================================================//

    current_messages.att_pos_mocap.time_usec = (uint64_t)0;
    current_messages.att_pos_mocap.q[0] = 1;
    current_messages.att_pos_mocap.q[1] = 0;
    current_messages.att_pos_mocap.q[2] = 0;
    current_messages.att_pos_mocap.q[3] = 0;
    if(count%20==10)
    {
        current_messages.att_pos_mocap.x = 0;
        current_messages.att_pos_mocap.y = 2;
        current_messages.att_pos_mocap.z = -2;
    }
    if(count%20==0)
    {
        current_messages.att_pos_mocap.x = 1;
        current_messages.att_pos_mocap.y = 3;
        current_messages.att_pos_mocap.z = -1;
    }

    mavlink_att_pos_mocap_t mocap= current_messages.att_pos_mocap;
    if(count%20==0||count%20==40)
        printf("POSITION XYZ = [ %.4f , %.4f , %.4f ] \n", mocap.x, mocap.y, mocap.z);

    mavlink_msg_att_pos_mocap_encode(system_id, component_id, &msg, &mocap);

    master->send_message(msg);
    usleep(40000);
    //=========================================================================//

    set_position(0.5, 2.5, -1.5, current_messages.set_position_target_local_ned);
    set_yaw(0, current_messages.set_position_target_local_ned);
    sp = current_messages.set_position_target_local_ned;
    sp.time_boot_ms = (uint32_t)0;
    sp.target_system = system_id;
    sp.target_component = component_id;

    //mavlink_message_t msg;
    mavlink_msg_set_position_target_local_ned_encode(system_id, component_id, &msg, &sp);
    master->send_message(msg);
    usleep(40000);
    //=========================================================================//

    mavlink_msg_att_pos_mocap_encode(system_id, component_id, &msg, &mocap);

    master->send_message(msg);
    usleep(40000);

    }

    // master->join();

    return 0;
}
//----------------------------------------------------------------------------//

移植到windows系统

分析：首先它里面用到了pipe，用来缓存数据，一来pipe不能直接移到windows，二来UDP本身有缓存机制，处理速度可以跟得上，所以这个去掉。socket是两个系统通用的东西，thread和mutex是c++11的新特性，所以需要移到visual studio 2015以上的vs版本。还有它由于用了pipe，所以需要两个线程，移到windows后只需要一个，读取后直接处理。

首先写一个简单的例程测试一下windows系统下UDP能否实现：

#include 
#include 
#include "mavlinklibrary/common/mavlink.h"
#pragma comment(lib,"ws2_32.lib")

void main()
{
    SOCKET socket1;

    //InitWinsock();
    WSADATA wsaData;
    int iErrorCode;
    if (WSAStartup(MAKEWORD(2, 1), &wsaData)) //调用Windows Sockets DLL
    {
        printf("Winsock无法初始化!\n");
        WSACleanup();
        return;
    }

    printf("服务器开始创建SOCKET。\n");
    struct sockaddr_in local;
    struct sockaddr_in from;
    int fromlen = sizeof(from);
    local.sin_family = AF_INET;
    local.sin_port = htons(14550); ///监听端口
    local.sin_addr.s_addr = INADDR_ANY; ///本机

    socket1 = socket(AF_INET, SOCK_DGRAM, 0);
    bind(socket1, (struct sockaddr*)&local, sizeof(local));

    mavlink_message_t message;
    mavlink_status_t status;

    while (1)
    {
        char buffer[1024] = "\0";
        //printf("waiting for message from others-------------\n");
        int nb_read = recvfrom(socket1, buffer, sizeof(buffer), 0, (struct sockaddr*)&from, &fromlen);
        if (nb_read != SOCKET_ERROR)
        {
            //printf("Received datagram from %s--%d\n", inet_ntoa(from.sin_addr), nb_read);
        }
        if (nb_read > 0)
        {
            for (int i = 0; i < nb_read; i++)
            {
                if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i],&message, &status))
                {
                    printf ("received message with id %d,sequence %d,from component %d of system %d!\n", message.msgid, message.seq, message.compid, message.sysid);
                    //在这里解析消息或者在另外的线程中处理。
                }
            }
        }
    }
    closesocket(socket1);
}

测试没有问题，然后开始移植。

最终移植结果

https://github.com/qianrenzhan/mavkitvs2015