（七）自定义MAVLink消息，并进行收发测试（Python）

  1.PX4开发环境配置过程中，gitsubmodule更新过程中，已将MAVLInk的文件下载，其中的消息格式定义在xml文件中，可通过MAVLink生成器生成对应版本和语言的库。（此处的v1.0并非指MAVLink版本）

cd ~/PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0/

2.在目录~/PX4-Autopilot/src/modules/mavlink/mavlink下，存在文件mavgenerate.py即为MAVLInk消息生成器。

3.在xml文件中自定义一条MAVLink消息，打开common.xml。

gedit ~/PX4_Firmware/src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml

下拉到文件最下方，模仿message的格式自定义一条消息，注意消息ID不要与现有的消息重合。

    <message id="12921" name="MY_MESSAGE">
      <description>My message.</description>
      <field type="uint8_t" name="contect" instance="true">Test </field>
    </message>

4.执行MAVLink生成器。

python3 ~/PX4_Firmware/src/modules/mavlink/mavlink/mavgenerate.py

选择刚刚修改的common.xml文件，输出路径可新建一个output文件夹，再手动在Out栏指定文件名mavlink.py，此处以生成Python语言的2.0版本为例，点击Generate。

5.检查生成的文件，搜索刚刚定义的消息名，可见已经生存对应的类。

6.发送，此处在文章《MAVLink系列——实战一：通过UDP发送MAVLink消息(python版本)》提供的代码下增加自定义消息，编写好以后，命名为send.py，与刚刚生成的mavlink.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)
    print("Send my_message {}-th".format(i))
    time.sleep(1)
    msg = MAVLink_my_message_message(255)
    mav.send(msg)
    print("Send heartbeat {}-th".format(i))

7.接收数据。

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

class DeviceUdp(object):
    """
    """
    def __init__(self, local_addr, target_addr=None):
        """
        local_port: local port to receive data
        For example, target_addr = ('192.168.4.1', 18750)
        """
        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)

    def read(self, size):
        buf, source_addr = self.dev.recvfrom(size)
        return buf

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

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

list_t = list()

while True:
    data = dev.read(256)
    msg = None

    if len(data)>0:
        try:
            msg = mav.parse_char(data)
        except Exception as e:
            continue

        if msg.get_srcSystem() == 1:
            if isinstance(msg, MAVLink_heartbeat_message):
                print("INFO: sys={}, cmp={}, msg_id={}".format(msg.get_srcSystem(), msg.get_srcComponent(),msg.get_msgId()))
                print(msg)
            else:
                print("INFO: sys={}, cmp={}, msg_id={}".format(msg.get_srcSystem(), msg.get_srcComponent(),msg.get_msgId()))
                print(msg)
                pass

7.运行测试。分别在2个终端运行send.py和read.py。

python3 send.py
python3 read.py # 另打开一个终端

可见发送端显示Send my_message x-th，接收端显示MY_MESSAGE {contect : 255}等内容，至此，自定义MAVLink消息收发测试成功。