PX4增加Mavlink消息回发

PX4通过Mavlink实现与地面站的通讯，在Mavlink中增加一个回发的信息的步骤如下：

1 在xml文件中新增消息定义
Mavlink关于消息的定义包含在Mavlink项目下的message_definitions中，可以在common.xml中增加需要的消息定义，如下

	...
	<message id="150" name="CUSTOM_STATUS">
		<description>Custom status</description>
		<field type="float" name="data1">Data 1</field>
		<field type="float" name="data2">Data 2</field>
		<field type="float" name="data3">Data 3</field>
	</message>
	...

这样就增加了一个message id为150，名称为CUSTOM_STATUS的消息。之后通过mavgenerate命令可以自动产生对应的消息定义和实现文件。

$ python3 -m mavgenerate

输入命令后会出现对话框，选择需要操作的xml文件以及目标目录，会在目标目录下产生的common目录，common目录下包含对应的消息定义文件common.h以及mavlink_msg_custom_status.h，里面包含对应消息结构体mavlink_custom_status_t的定义

上述的消息定义也可以在另外的xml文件来定义，这样就会产生另外的目录，以及对应的消息定义文件。

2 定义一个MavlinkStream类
在PX4项目的src/modules/mavlink目录下的mavlink_messages.cpp中定义一个基于MavlinkStream的类，以实现消息操作和回发，可以参考其它已有的MavlinkStream类，大致如下

class MavlinkStreamCustomStatus : public MavlinkStream {
public:
	const char *get_name() const {
		return MavlinkStreamCustomStatus::get_name_static();
	}
	static const char *get_name_static() {
		return "CUSTOM_STATUS";
	}

	static uint16_t get_id_static()	{
		return MAVLINK_MSG_ID_CUSTOM_STATUS;
	}

	...
	bool send(const hrt_abstime t) {

	...
}

其中send函数是主要需要实现的函数，表示在消息回发时需要进行的操作。如果我们需要在每次回发时订阅一个orb消息的数据进行回发，示例实现如下

class MavlinkStreamCustomStatus : public MavlinkStream {
	...

	MavlinkOrbSubscription *_radar_data_sub;
	uint64_t _radar_data_time;
	...
protected:
	explicit MavlinkStreamCustomStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
		_radar_data_sub(_mavlink->add_orb_subscription(ORB_ID(radar_data))),
		_radar_data_time(0)
	{}
	
	bool send(const hrt_abstime t)
	{
		mavlink_custom_status_t msg = {};
		radar_data_s radar_data = {};
		
		if (_radar_data_sub->update(&_radar_data_time, &radar_data)) {
			msg.data1 = radar_data.sector[0];
			msg.data2 = radar_data.sector[1];
			msg.data3 = radar_data.sector[2];

			mavlink_msg_custom_status_send_struct(_mavlink->get_channel(), &msg);
			return true;
		}

		return false;
	}
	
	...
}

在类中定义了需要订阅的消息和更新的时间

MavlinkOrbSubscription *_radar_data_sub;
uint64_t _radar_data_time;

在构造函数中赋予_radar_data_sub初值

_mavlink->add_orb_subscription(ORB_ID(radar_data))

表示对orb消息radar_data的订阅。
在send函数中，运行update函数

_radar_data_sub->update(&_radar_data_time, &radar_data)

更新消息的订阅，如果存在新的消息，则消息数据存放于radar_data，并且返回true。然后进行发送的操作，把收到的数据填充到custom status消息结构中，并调用mavlink_msg_custom_status_send_struct函数进行发送。

3 配置Mavlink消息发送
在mavlink模块目录下的mavlink_main.cpp文件中的Mavlink::task_main函数中配置消息的回发，大致如下

int
Mavlink::task_main(int argc, char *argv[])
{
	...
	switch (_mode) {
	case MAVLINK_MODE_NORMAL:
		configure_stream("ALTITUDE", 1.0f);
		configure_stream("ATTITUDE", 20.0f);
		...
		configure_stream("CUSTOM_STATUS", 2.0f);
		...
		break;
	case MAVLINK_MODE_ONBOARD:
		...
	}

	...
}

在进行一段初始化之后，根据_mode值进行mavlink stream的配置，其中_mode的值表示不同的通讯模式，比如在MAVLINK_MODE_NORMAL中，包含很多消息的配置，其中配置CUSTOM_STATUS消息

configure_stream("CUSTOM_STATUS", 2.0f);

表示按照2Hz频率回发该消息。

配置完成之后，进行编译，然后程序运行时会按照设定的频率定时调用前面MavlinkStream的send函数，实现消息的定时回发。