MAVLink 实战一：通过UDP发送MAVLink消息

诗筱涵

已于 2024-05-10 18:38:16 修改

阅读量1.4k

点赞数 2

分类专栏：无人机-mavros控制PX4 offboard 无人机-mavlink 文章标签： mavlink

于 2024-05-08 15:20:48 首次发布

原文链接：https://zhuanlan.zhihu.com/p/415277586

版权

无人机-mavros控制PX4 offboard 同时被 2 个专栏收录

137 篇文章

订阅专栏

无人机-mavlink

45 篇文章

订阅专栏

转载自 MAVLink系列——实战一：通过UDP发送MAVLink消息(python版本) - 知乎

MAVLink 实战一：通过UDP发送MAVLink消息

参考

MAVLink GitHub源码地址：https://github.com/mavlink/mavlink

MAVLink官方文档：https://mavlink.io/en/messages/common.html

本节内容

学习python UDP编程
学习QGC的简单使用
实现基于UDP的MAVLink消息发送

所有代码都在gitee上：https://gitee.com/wujinDrone/mavlink_tutorial#https://www.yuque.com/u21969096/cez3q8

环境配置说明

软硬件

我们这边的所有编程都是基于Ubuntu（18.04），请准备好装有Ubuntu环境计算机。我们编程使用的python3.6，Ubuntu已经默认自带了。

请提前安装好QGC，具体安装方法参考官网说明。

MAVLink库代码

根据上一节，MAVLink库的生成说明，生成一个V2版本的Python库，重名名为mavlink.py文件并放到一个文件夹下如mavlink_learn，然后在同级目录下新建一个mavcrc.py文件，填入如下代码：

'''
MAVLink X25 CRC code
Copyright Andrew Tridgell
Released under GNU LGPL version 3 or later
'''
from builtins import object
class x25crc(object):
    '''x25 CRC - based on checksum.h from mavlink library'''
    def __init__(self, buf=None):
        self.crc = 0xffff
        if buf is not None:
            if isinstance(buf, str):
                self.accumulate_str(buf)
            else:
                self.accumulate(buf)
    def accumulate(self, buf):
        '''add in some more bytes'''
        accum = self.crc
        for b in buf:
            tmp = b ^ (accum & 0xff)
            tmp = (tmp ^ (tmp<<4)) & 0xFF
            accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
        self.crc = accum
    def accumulate_str(self, buf):
        '''add in some more bytes'''
        accum = self.crc
        import array
        bytes = array.array('B')
        bytes.fromstring(buf)
        self.accumulate(bytes)

现在我们当前文件夹目录结构如下：

|--mavlink_learn/
    |-- mavlink.py
    |-- mavcrc.py

最后，你需要修改下mavlink.py文件中mavrcrc的导入路径，只需要将第13行：

from ...generator.mavcrc import x25crc

更改为：

from mavcrc import x25crc

即可。

UDP基础

UDP是面向无连接的不可靠数据传输，不想TCP那样有三次握手，UDP只负责发送，不负责确认对方是否收到数据，也没有重传机制，这些都需要用户自己实现。但UDP简单好用，例如基于PX4的gazebo仿真就用到UDP作为通信方式。

python UDP通信是通过导入socket包来实现。

发送

函数: sendto(data, addr)

其中data为字节数组，如b'hello world'，addr为地址和端口组成的tuple，例如('127.0.0.1', 14550)。

接收

函数: data = recvfrom(len)

其中len为接收长度，例如len=1024即最大读取1024个字节，返回值为字节数组。

UDP客户端-服务端通信

首先我们编写一个简单的客户端-服务端的通信示例：

服务端代码udp_server.py为：

import socket
import time
BUFSIZE = 1024
def main():
    ip_port = ('', 14550)
    server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    server.bind(ip_port)

    while True:
        data, addr = server.recvfrom(BUFSIZE)
        print('Received frome {}:'.format(addr))
        print(data)
        response = b"Hello, this server1, I received: "
        server.sendto(response + data, addr)
    server.close()
if __name__ == '__main__':
    main()

客户端代码udp_client.py为：

import socket
import time
BUFSIZE = 1024
def main():
    client = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
    ip_port = ('127.0.0.1', 14550)
    client.sendto(b"hello this is udp client",ip_port)
    data, addr = client.recvfrom(BUFSIZE)
    print("Received from: {}".format(addr))
    print(data)
    client.close()

if __name__ == '__main__':
    main()

先运行在一个终端运行服务端，然后打开另外一个终端运行客户端（连续运行两次），有如下结果：

# 服务端
$ python3 udp_server.py 
Received frome ('127.0.0.1', 45174):
b'hello this is udp client'
Received frome ('127.0.0.1', 51441):
b'hello this is udp client'

# 客户端
$ python3 udp_client.py 
Received from: ('127.0.0.1', 14550)
b'Hello, this server1, I received: hello this is udp client'
$ python3 udp_client.py 
Received from: ('127.0.0.1', 14550)
b'Hello, this server1, I received: hello this is udp client'

UDP服务端-服务端通信

通过客户端编程我们可以看到，客户端没有绑定本地端口，发送时只要指定发送的目的IP和Port即可，将随机绑定一个本地端口。故上面的结果也显示，启动两次客户端，服务端接收的端口是不一样的！

而很多时候，我们进行UDP通信时，两边都需要绑定本地端口！这个时候我们可以采用服务端和服务端通信方式！

新建一个服务端，命名为udp_server2.py，添加如下代码：

import socket
import time

BUFSIZE = 1024
def main():
    ip_port = ('', 14540)
    server2 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    server2.bind(ip_port)
    des_addr = ('127.0.0.1', 14550)
    while True:
        data_send = b"Hello, this server2: "
        server2.sendto(data_send, des_addr)

        data, addr = server2.recvfrom(BUFSIZE)
        print('Recived from {}:'.format(addr))
        print(data)
        time.sleep(1)

    server2.close()
if __name__ == '__main__':
    main()

先运行服务端1，然后再启动服务端2，有：

# 服务端1
$ python3 udp_server.py 
Received frome ('127.0.0.1', 14540):
b'Hello, this server2: '
Received frome ('127.0.0.1', 14540):
b'Hello, this server2: '

# 服务端2
$ python3 udp_server2.py 
Recived from ('127.0.0.1', 14550):
b'Hello, this server1, I received: Hello, this server2: '
Recived from ('127.0.0.1', 14550):
b'Hello, this server1, I received: Hello, this server2: '

发送MAVLink消息

在一开始学习时，特别涉及到通信编程，为了保证至少有一端是完全没问题的，我们最好采用现有的工具作为接收或发送端！不然出现无法通信的情况，不好确定你的代码到底是接收端有问题还是发送端写的有问题。

这里我们使用QGC作为通信的一端，用于接收数据，接下来我们编写MAVLink数据的发送端部分！

QGC设置

启动QGC

$ ./Desktop/QGroundControl.AppImage

新建一个通信连接

打开主菜单，选择Application Settings->Comm Links，新建一个UDP连接，本地端口填写14550，服务端地址填写127.0.0.1:14540。

创建完成后选择这个连接，然后点击Connect，这样QGC将会等等连接。

注意，如果启动后，不建立这个连接也没关系，因为这个连接对应的本地端口和远程服务端口比较特殊，是PX4飞控和QGC通信的默认端口，14550会一直在QGC监听中，收到消息会自动处理！

连接QGC

为了与QGC建立一个通信，我们创建一个“无人机”，这个“无人机”当然不是一个真实的，但能够模仿无人机发送一些基本数据，最核心的数据就是呼吸包——Heartbeat消息，这个消息包含了无人机基本信息如类型、id、模式等等。

我们新建一个测试文件test_sim_drone_udp_send.py，添加如下代码：

#!/usr/bin/env python3
import time
import socket
from mavlink import *

class DeviceUdp(object):
    """
    """
    def __init__(self, local_addr, target_addr=None):
        self.dev         = None
        self.local_addr  = local_addr
        self.target_addr = target_addr
        self.dev = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.dev.bind(self.local_addr)

    def write(self, buf):
        send_callback = self.dev.sendto(buf, self.target_addr)

local_addr  = ('', 14540)
target_addr = ('127.0.0.1', 14550)
dev = DeviceUdp(local_addr, target_addr)

sys_id = 1
cmp_id = 1
mav = MAVLink(dev, sys_id, cmp_id)
for i in range(10):
    msg = MAVLink_heartbeat_message(MAV_TYPE_QUADROTOR, 
            MAV_AUTOPILOT_PX4, MAV_MODE_PREFLIGHT, 0x60000, MAV_STATE_STANDBY, 2)
    mav.send(msg)
    time.sleep(1)
    print("Send heartbeat {}-th".format(i))

接下来运行这个代码：

$ python3 test_sim_drone_udp_send.py

然后便能够听到QGC建立无人机连接的提示声音，然后在主菜单打开Analyze Tools->MAVLink Inspector后，可以看到已经收到了HEARTBEAT消息！

代码详解

1、MAVLink类

MAVLink类辅助处理MAVLink通信，包括消息的发送、解析。其__init__函数重要部分如下：

def __init__(self, file, srcSystem=0, srcComponent=0, use_native=False):
                self.seq = 0
                self.file = file
                self.srcSystem = srcSystem
                self.srcComponent = srcComponent

file: 表示实现底层信息传输如网络传输、串口传输的对象，由于MAVLink不负责底层硬件如网卡、串口的操作，所有需要使用者给定。这个对象必须有一个write成员函数，用来实现字节数组的发送！

我们把这个file当做是一个负责底层数据传输的设备！

srcSystem: 指定本机的system_id，这样接收者才能知道本地的“身份信息”；
srcComponent：指定本机的component_id，这样接收者才能知道本地的“身份信息”；

所以以下三行代码就是实例化一个MAVLink对象：

sys_id = 1
cmp_id = 1
mav = MAVLink(dev, sys_id, cmp_id)

2、DeviceUdp类

这个是使用UDP进行底层数据传输的设备！

这个类是用户必须实现的，它定义了底层数据传输的接口，这里我们选择了UDP传输，故这个类就是实现了UDP连接的初始化以及数据发送接口函数——write，这个函数只需要一个参数即可，就是字节数组buf，然后通过UDP的sendto方法将这个数组发送出去！

def write(self, buf):
        send_callback = self.dev.sendto(buf, self.target_addr)

3、消息生成

根据使用者的意图，需要找到对应的MAVLink消息，每个消息（因平台特殊定义的除外）的内容都可以在官网或者下载的源码mavlink-master/message_definitions/v1.0/common.xml或者minimal.xml中找到具体定义。

MAVLink库将每个消息都进行了类实现！例如HEARTBEAT ( #0 )对应的类就是MAVLink_heartbeat_message，我们在进行消息生成时就是实例化了消息类而已，根据消息定义填入参数即可！

msg = MAVLink_heartbeat_message(MAV_TYPE_QUADROTOR, 
            MAV_AUTOPILOT_PX4, MAV_MODE_PREFLIGHT, 0x60000, MAV_STATE_STANDBY, 2)

4、消息发送

MAVLink提供了send(mavmsg)成员函数用于发送消息，如下：

mav.send(msg)

还记得我们前面说过，消息的底层发送需要使用者来实现吗？

前面我们创建了DeviceUdp类并且创建了write方法，其实在调用mav.send(msg)时，send函数会调用DeviceUdp.write，从而实现了数据传输！

############# mavlink.py ###############
class MAVLink(object):
    def __init__(self, file, srcSystem=0, srcComponent=0, use_native=False):
        self.file = file
    def send(self, mavmsg, force_mavlink1=False):
                '''send a MAVLink message'''
                buf = mavmsg.pack(self, force_mavlink1=force_mavlink1)
                ### 调用使用者定义的设备类的write方法进行传输
                self.file.write(buf)
                ###
############# test_sim_drone_udp_send.py ###############
mav = MAVLink(dev, sys_id, cmp_id)

发布于 2021-09-29 10:15