PX4模块设计之五：自定义MAVLink消息

基于PX4开源软件框架简明简介的框架设计，逐步分析内部模块功能设计。

MAVLink消息不像uORB消息，MAVLink(Micro Air Vehicle Link，微型空中飞行器链路通讯协议)是实际上一种链路通信协议，而在实际飞控中使用的时候通常基于无线硬件：433MHz的串行通信模块(UART) 或者 WiFi通信模块(UDP)等。

前面章节我们介绍了MAVLink简介，接下去我们介绍自定义MAVLink消息，因为这个才是实际开发过程中需要经常用到的，也就是将GCS地面站控制信息发送到飞控或者从飞控获得消息。

注：当然如果有伴飞电脑，两者之间的通信也可以采用MAVLink。不过MAVLink最初设计的时候是应用于低带宽，链路不稳定的场景。伴飞电脑通常是处理稳定高带宽的数据，比如：视频流等。

1. MAVLink Dialects

这里有必要首先提出Dialects的概念，可以认为是一种飞控与地面站之间的MAVLink协议。不同的飞控系统所使用的Dialects是不完全一样的。

根据MAVLink官网给出消息定义，目前主要是如下几种Dialects:

1. ASLUAV.xml
2. AVSSUAS.xml
3. all.xml
4. ardupilotmega.xml
5. icarous.xml
6. matrixpilot.xml
7. paparazzi.xml
8. python_array_test.xml
9. storm32.xml
10. uAvionix.xml
11. ualberta.xml

根据/mavlink/messages/1.0和源代码目录整理内容如下：

目录结构  //对应上面官网提供的Dialects种类
.
├── ASLUAV.xml
├── AVSSUAS.xml
├── ardupilotmega.xml
├── icarous.xml
├── matrixpilot.xml
├── paparazzi.xml
├── python_array_test.xml
├── storm32.xml
├── uAvionix.xml
├── ualberta.xml

/* 分隔，用于解释说明：标准或者核心定义，目前看standard和common的概念有点模糊，代码是standard包含common，而官方定义是common包含standard；详见下面两个Dialects的分析 */

├── minimal.xml    //the minimum set of entities (messages, enums, MAV_CMD) required to set up a MAVLink network.
├── standard.xml   
├── common.xml     

/* 分隔，用于解释说明 */

├── test.xml      //Test XML definition file.
├── all.xml       //This includes all other XML files, and is used to verify that there are no ID clashes (and can potentially be used by GCS to communicate with any core dialect).

/* 分隔，用于解释说明 */

└── development.xml

0 directories, 16 files

注：接下来我们针对比较熟悉的两个Dialects进行整理。

1.1 PX4 Dialects

development.xml version=0
├──> include standard.xml
│ └──> include common.xml
│ └──> include minimal.xml
├──> enums
│ ├──> WIFI_NETWORK_SECURITY
│ ├──> AIRSPEED_SENSOR_TYPE
│ ├──> PARAM_TRANSACTION_TRANSPORT
│ ├──> PARAM_TRANSACTION_ACTION
│ ├──> MAV_STANDARD_MODE
│ └──> MAV_CMD
└──> messages
├──> 19 PARAM_ACK_TRANSACTION
├──> 52 MISSION_CHANGED
├──> 53 MISSION_CHECKSUM
├──> 295 AIRSPEED
├──> 298 WIFI_NETWORK_INFO
├──> 361 FIGURE_EIGHT_EXECUTION_STATUS
├──> 396 COMPONENT_INFORMATION_BASIC
├──> 414 GROUP_START
├──> 415 GROUP_END
├──> 435 AVAILABLE_MODES
└──> 436 CURRENT_MODE

1.2 Paprazzi Dialects

paparazzi.xml version=3
├──> include standard.xml
│ └──> include common.xml
│ └──> include minimal.xml
└──> messages
├──> 180 SCRIPT_ITEM
├──> 181 SCRIPT_REQUEST
├──> 182 SCRIPT_REQUEST_LIST
├──> 183 SCRIPT_COUNT
└──> 184 SCRIPT_CURRENT

1.3 MAVLink XML File Format

• include: 引用其他XML定义
• version: Dialects的版本信息，用于HEARTBEAT中的mavlink_version字段
• dialect: TBD
• enums: Dialects的特定枚举值
• messages: Dialects的特定消息

注1：当XML抽象及标准后，就升级到了类似minimal.xml/standard.xml/common.xml的位置。
注2：基于这种概念，我认为官网给出的common包含standard再包含minimal是比较合适的（实际代码并非如此）。

<?xml version="1.0"?>
<mavlink>

    <include>common.xml</include>
    <include>other_dialect.xml</include>

    <!-- NOTE: If the included file already contains a version tag, remove the version tag here, else uncomment to enable. -->
    <!-- <version>6</version> -->

    <dialect>8</dialect>

    <enums>
        <!-- Enums are defined here (optional) -->
    </enums>

    <messages>
        <!-- Messages are defined here (optional) -->
    </messages>

</mavlink>

2. 添加自定义MAVLink消息

注：这里我们借用PX4的例子src/examples/px4_mavlink_debug，但是我们会将每个步骤都清清楚楚明明白白的给大家整理出来。

2.1 步骤1：确认飞控代码使用的MAVLink Dialects

默认配置使用/src/modules/mavlink/CMakeLists.txt文件内的变量MAVLINK_DIALECT定义为：development

34 set(MAVLINK_DIALECT "development") # standard, development, etc

因此，代码将会使用的是/mavlink/messages/1.0/下的development配置文件development.xml

2.2 步骤3：新增uORB自定义消息

该步骤可参考PX4模块设计之三：自定义uORB消息。

注：这里使用了一个例子，我们给出找到的位置和代码msg/debug_vect.msg。

1 uint64 timestamp	# time since system start (microseconds)
2 char[10] name           # max. 10 characters as key / name
3 float32 x               # x value
4 float32 y               # y value
5 float32 z               # z value

2.3 步骤3：建立uORB与MAVLink消息映射关系

MAVLink message	uORB topic
DEBUG_VECT	debug_vect

2.4 步骤4：新增MAVLink自定义XML消息

注：这里使用了一个例子，我们给出找到的位置和代码src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml。

6521    <message id="250" name="DEBUG_VECT">
6522      <description>To debug something using a named 3D vector.</description>
6523      <field type="char[10]" name="name" instance="true">Name</field>
6524      <field type="uint64_t" name="time_usec" units="us">Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.</field>
6525      <field type="float" name="x">x</field>
6526      <field type="float" name="y">y</field>
6527      <field type="float" name="z">z</field>
6528    </message>

2.5 步骤5：新增MAVLink自定义MavlinkStreamDebugVect消息类

注：这里使用了一个例子，我们给出找到的位置和代码src/modules/mavlink/streams/DEBUG_VECT.hpp

/****************************************************************************
 *
 *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#ifndef DEBUG_VECT_HPP
#define DEBUG_VECT_HPP

#include <uORB/topics/debug_vect.h>

class MavlinkStreamDebugVect : public MavlinkStream
{
public:
	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDebugVect(mavlink); }

	static constexpr const char *get_name_static() { return "DEBUG_VECT"; }
	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_DEBUG_VECT; }

	const char *get_name() const override { return get_name_static(); }
	uint16_t get_id() override { return get_id_static(); }

	unsigned get_size() override
	{
		return _debug_sub.advertised() ? MAVLINK_MSG_ID_DEBUG_VECT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
	}

private:
	explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {}

	uORB::Subscription _debug_sub{ORB_ID(debug_vect)};

	bool send() override
	{
		debug_vect_s debug;

		if (_debug_sub.update(&debug)) {
			mavlink_debug_vect_t msg{};
			msg.time_usec = debug.timestamp;
			memcpy(msg.name, debug.name, sizeof(msg.name));
			msg.name[sizeof(msg.name) - 1] = '\0'; // enforce null termination
			msg.x = debug.x;
			msg.y = debug.y;
			msg.z = debug.z;

			mavlink_msg_debug_vect_send_struct(_mavlink->get_channel(), &msg);

			return true;
		}

		return false;
	}
};

#endif // DEBUG_VECT_HPP

2.5 步骤5：新增MavlinkStreamDebugVect类到mavlink_messages.cpp

注：这里使用了一个例子，我们给出找到的位置和代码src/modules/mavlink/mavlink_messages.cpp

127 # include "streams/DEBUG_VECT.hpp"

466 #if defined(DEBUG_VECT_HPP)
467 	create_stream_list_item<MavlinkStreamDebugVect>(),
468 #endif // DEBUG_VECT_HPP

2.6 步骤6：完成debug_vect的uORB自定义消息发布DEMO

该步骤可参考PX4模块设计之三：自定义uORB消息。

注：这里使用了一个例子，我们给出找到的位置和代码src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp，这里使用的是内部模块，所以相对来说工程更加简单。

/****************************************************************************
 *
 *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file px4_mavlink_debug.cpp
 * Debug application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/px4_config.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <poll.h>

#include <systemlib/err.h>
#include <drivers/drv_hrt.h>

#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/debug_array.h>

extern "C" __EXPORT int px4_mavlink_debug_main(int argc, char *argv[]);

int px4_mavlink_debug_main(int argc, char *argv[])
{
	printf("Hello Debug!\n");

	/* advertise named debug value */
	struct debug_key_value_s dbg_key;
	strncpy(dbg_key.key, "velx", 10);
	dbg_key.value = 0.0f;
	orb_advert_t pub_dbg_key = orb_advertise(ORB_ID(debug_key_value), &dbg_key);

	/* advertise indexed debug value */
	struct debug_value_s dbg_ind;
	dbg_ind.ind = 42;
	dbg_ind.value = 0.5f;
	orb_advert_t pub_dbg_ind = orb_advertise(ORB_ID(debug_value), &dbg_ind);

	/* advertise debug vect */
	struct debug_vect_s dbg_vect;
	strncpy(dbg_vect.name, "vel3D", 10);
	dbg_vect.x = 1.0f;
	dbg_vect.y = 2.0f;
	dbg_vect.z = 3.0f;
	orb_advert_t pub_dbg_vect = orb_advertise(ORB_ID(debug_vect), &dbg_vect);

	/* advertise debug array */
	struct debug_array_s dbg_array;
	dbg_array.id = 1;
	strncpy(dbg_array.name, "dbg_array", 10);
	orb_advert_t pub_dbg_array = orb_advertise(ORB_ID(debug_array), &dbg_array);

	int value_counter = 0;

	while (value_counter < 100) {
		uint64_t timestamp_us = hrt_absolute_time();

		/* send one named value */
		dbg_key.value = value_counter;
		dbg_key.timestamp = timestamp_us;
		orb_publish(ORB_ID(debug_key_value), pub_dbg_key, &dbg_key);

		/* send one indexed value */
		dbg_ind.value = 0.5f * value_counter;
		dbg_ind.timestamp = timestamp_us;
		orb_publish(ORB_ID(debug_value), pub_dbg_ind, &dbg_ind);

		/* send one vector */
		dbg_vect.x = 1.0f * value_counter;
		dbg_vect.y = 2.0f * value_counter;
		dbg_vect.z = 3.0f * value_counter;
		dbg_vect.timestamp = timestamp_us;
		orb_publish(ORB_ID(debug_vect), pub_dbg_vect, &dbg_vect);

		/* send one array */
		for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
			dbg_array.data[i] = value_counter + i * 0.01f;
		}

		dbg_array.timestamp = timestamp_us;
		orb_publish(ORB_ID(debug_array), pub_dbg_array, &dbg_array);

		warnx("sent one more value..");

		value_counter++;
		px4_usleep(500000);
	}

	return 0;
}

3. 测试验证效果

3.1 编译+运行

$ make px4_sitl jmavsim

pxh> px4_mavlink_debug 

Hello Debug!
WARN  [px4_mavlink_debug] sent one more value..
WARN  [px4_mavlink_debug] sent one more value..
WARN  [px4_mavlink_debug] sent one more value..
... 

3.2 QGroundControl查看

按照PX4开发环境搭建–模拟器编译及QGroundControl & RC遥控模拟配置方式链接sitl模拟飞控。

然后，选择Analyze Tools

再选择MAVLink Inspector

message是按照字母序排列的，没有DEBUG相关的消息信息。

当运行px4_mavlink_debug程序，我们可以看到DEBUG_VECT消息收到。

4. 参考资料

【1】MAVLink Messaging
【2】MAVLink Developer Guide
【3】MAVLink XML File Schema/Format