MAVLink 飞控通讯协议开发详解

目录

前言

一、MAVLink 飞控通讯协议 简介

二、ubuntu系统下开发流程

1、下载MAVLink开发工具

2、生成消息集的MAVLink 库文件

3、理解MAVLink报文格式和通用消息集字段含义

4、编写示例，使用MAVLink库文件接口打包或解析报文

---

前言

之前的文章中介绍过PX4开源飞控的开发过程，在仿真环节通过QGC地面站控制无人机或通过UDP服务器接收无人机数据：

PX4开源飞控--开发环境搭建 编译仿真及烧录

本章将介绍无人机和地面站之间的通讯协议（MAVLink飞控通讯协议）、开发流程、报文格式和通用消息集，方便后期操控无人机。

一、MAVLink 飞控通讯协议 简介

        MAVLink飞控通讯协议为一种设计用于资源受限系统及带宽受限链路的二进制遥测协议。常用于无人机和地面站之间的通讯，并已集成于API，工程及软件包中。  例如，APM、PIXHAWK飞控，MP、QGC地面站均使用了MAVLink协议进行通讯。  目前，主要发行了两个版本: v1.0 和 v2.0 ，这两个版本向后兼容 （即 v2.0 可以分析和发送 v1.0 数据包)。   

提供MAVLink官方指南：mavlink指南

MAVLink通讯协议主要是对报文进行组包，并不限定传输通信方式，一般开发流程为：
1、下载MAVLink开发工具

2、生成消息集的MAVLink 库文件
MAVLink提供了通用消息集的消息文件（XML格式），如果开发者需要自定义一些无人机和地面站之间的消息，就需要编写自己的消息文件。然后，使用代码生成器将消息文件生成不同编程语言使用的MAVLink库文件。

3、理解MAVLink报文格式和通用消息集的字段含义

4、使用MAVLink库文件的接口对报文进行打包或解析
编写一个C语言的UDP通信传输示例，接收解析来自PX4软件的数据包，再返回信息给PX4。
后续可将MAVLink库文件可以移植到开发项目（如PX4、QGC代码）中使用。

二、ubuntu系统下开发流程

        对于通用消息集，MAVLink直接提供基于C/C++语言的MAVLink库，也可以通过代码生成器将通用消息集的消息文件生成不同编程语言的库文件。但是，对于一些开发者自定义消息，就需要通过代码生成器生成对应消息的库文件了。因此，建议通过代码生成器生成。

1、下载MAVLink开发工具

• 下载安装Python环境：

sudo apt install python3-pip
pip3 install --user future
sudo apt install python3-lxml libxml2-utils
sudo apt install python3-tk

• 下载MAVLink开发工具：

git clone https://github.com/mavlink/mavlink.git --recursive
cd mavlink
mkdir out

2、生成消息集的MAVLink 库文件

提供图形界面和命令行方式生成，建议采用图形界面。

（1）使用图形界面方式：

打开代码生成器的图形用户界面，执行：

python3 -m mavgenerate

• 选则某个消息配置文件（例如包括了全部消息的：mavlink/message_definitions/1.0/all.xml）

        通用消息集的文件在message_definitions/v1.0/ 目录下，如果需要自定义消息，可以参考这些文件进行编写，然后放到此目录中。在此提供编写指南：消息文件编写

• 选择输出路径(例如mavlink/out)
• 选择编程语言(例如C)
• MAVLink版本(建议2.0)
• 选中验证和/或验证单位，以验证XML规范（两个都打勾）
• 点击Generate （生成后，在out目录下可看到相关头文件）

（2）使用命令行方式：
示例，生成MAVLink2.0版本的C语言库

python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=out/ message_definitions/v1.0/all.xml

3、理解MAVLink报文格式和通用消息集字段含义

MAVLink报文格式，参照官方说明：报文格式
MAVLink 2 的数据包格式：

通用消息集的字段含义，参照官方说明：通用消息集

4、编写示例，使用MAVLink库文件接口打包或解析报文

examples/c/my_udp_example.c 是本人修改之后的MAVLink UDP通信的简单C示例，接收解析来自PX4飞控软件的数据。

examples/c/my_udp_example.c

// Simple example receiving and sending MAVLink v2 over UDP
// based on POSIX APIs (e.g. Linux, BSD, macOS).

#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>

#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <time.h>
#include<sys/time.h>

#include "common/mavlink.h"


void receive_some(int socket_fd, struct sockaddr_in* src_addr, socklen_t* src_addr_len, bool* src_addr_set);
void handle_heartbeat(const mavlink_message_t* message);

void send_some(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len);
void send_heartbeat(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len);


int main(int argc, char* argv[])
{
    // Open UDP socket
    const int socket_fd = socket(PF_INET, SOCK_DGRAM, 0);

    if (socket_fd < 0) {
        printf("socket error: %s\n", strerror(errno));
        return -1;
    }

    // Bind to port
    const int port = 14540;
    struct sockaddr_in addr = {};
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    inet_pton(AF_INET, "0.0.0.0", &(addr.sin_addr)); // listen on all network interfaces
   // addr.sin_port = htons(port); // default port on the ground
addr.sin_port = htons(port);
printf("udp server (port=%d)\n", port);


    if (bind(socket_fd, (struct sockaddr*)(&addr), sizeof(addr)) != 0) {
        printf("bind error: %s\n", strerror(errno));
        return -2;
    }

    // We set a timeout at 100ms to prevent being stuck in recvfrom for too
    // long and missing our chance to send some stuff.
    struct timeval tv;
    tv.tv_sec = 0;
    tv.tv_usec = 100000;
    if (setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
        printf("setsockopt error: %s\n", strerror(errno));
        return -3;
    }

    struct sockaddr_in src_addr = {};
    socklen_t src_addr_len = sizeof(src_addr);
    bool src_addr_set = false;

    while (true) {
        // For illustration purposes we don't bother with threads or async here
        // and just interleave receiving and sending.
        // This only works  if receive_some returns every now and then.
        receive_some(socket_fd, &src_addr, &src_addr_len, &src_addr_set);

        if (src_addr_set) {
            send_some(socket_fd, &src_addr, src_addr_len);
        }
    }

    return 0;
}

void receive_some(int socket_fd, struct sockaddr_in* src_addr, socklen_t* src_addr_len, bool* src_addr_set)
{
    // We just receive one UDP datagram and then return again.
    char buffer[2048]; // enough for MTU 1500 bytes

    const int ret = recvfrom(
            socket_fd, buffer, sizeof(buffer), 0, (struct sockaddr*)(src_addr), src_addr_len);

    if (ret < 0) {
	if (errno == EAGAIN || errno == EWOULDBLOCK) {
	    return;	
	} else {
            printf("recvfrom error: %s\n", strerror(errno));
	}
    } else if (ret == 0) {
        // peer has done an orderly shutdown
        return;
    }

   for (int i = 0; i < ret; i++) {
	printf("%02x ", buffer[i]);
   } 
   printf("\n");

    *src_addr_set = true;

    mavlink_message_t message;
    mavlink_status_t status;
    for (int i = 0; i < ret; ++i) {
        if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &message, &status) == 1) {

            // printf(
            //     "Received message %d from %d/%d\n",
            //     message.msgid, message.sysid, message.compid);

            switch (message.msgid) {
            case MAVLINK_MSG_ID_HEARTBEAT:
                handle_heartbeat(&message);
                break;
            }
        }
    }
}

void handle_heartbeat(const mavlink_message_t* message)
{
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(message, &heartbeat);

    printf("Got heartbeat from ");
    switch (heartbeat.autopilot) {
        case MAV_AUTOPILOT_GENERIC:
            printf("generic");
            break;
        case MAV_AUTOPILOT_ARDUPILOTMEGA:
            printf("ArduPilot");
            break;
        case MAV_AUTOPILOT_PX4:
            printf("PX4");
            break;
        default:
            printf("other");
            break;
    }
    printf(" autopilot\n");
}

void send_some(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len)
{
    // Whenever a second has passed, we send a heartbeat.
    static time_t last_time = 0;
    time_t current_time = time(NULL);
    if (current_time - last_time >= 1) {
        send_heartbeat(socket_fd, src_addr, src_addr_len);
        last_time = current_time;
    }
}

void send_heartbeat(int socket_fd, const struct sockaddr_in* src_addr, socklen_t src_addr_len)
{
    mavlink_message_t message;

    const uint8_t system_id = 42;
    const uint8_t base_mode = 0;
    const uint8_t custom_mode = 0;
    mavlink_msg_heartbeat_pack_chan(
        system_id,
        MAV_COMP_ID_PERIPHERAL,
        MAVLINK_COMM_0,
        &message,
        MAV_TYPE_GENERIC,
        MAV_AUTOPILOT_GENERIC,
        base_mode,
        custom_mode,
        MAV_STATE_STANDBY);

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    const int len = mavlink_msg_to_send_buffer(buffer, &message);

    int ret = sendto(socket_fd, buffer, len, 0, (const struct sockaddr*)src_addr, src_addr_len);
    if (ret != len) {
        printf("sendto error: %s\n", strerror(errno));
    } else {
        printf("Sent heartbeat\n");
    }
}

编译运行：

#编译
gcc -std=c99 -Wno-address-of-packed-member -Wno-cast-align -I ../../out/ -o mavlink_udp my_udp_example.c

#运行
./mavlink_udp