Mavlink地面站编写之八–MAVLINK消息自定义
PIXHAWk MAVLINK消息自定义
对于PIXHAWK这个系统来讲，MAVLINK是个小型的数据通信协议，负责地面站和飞控本身的数据交互和地面站向飞控发送数据指令。前面的文章已经讲过MAVLINK这个数据格式解析的分析，这里不做讲解。因为我们在使用基于这套飞控开发的过程中，会有很多自定义的数据要求，比如我们添加一个新的传感器(在飞控中添加一个自定义传感器，具体请看相关章节)，我们会把新的传感器数据发送回来和地面站交互，这时候就会涉及到自定义一个MAVLINK的消息包，自定义消息包的数据，发送给地面站来解析显示。对于MAVLINK这套系统，有对应的工具，来自动生成MAVLINK的库函数，只要约定好消息包的结构体，我们就可以用相应的MAVLINK工具，来生成MAVLINK库的头文件和功能接口函数。
使用MAVLINK GENERATOR(mavlink生成器) 基于python的，下载即可。
Firmware\mavlink\include\mavlink\v1.0\message_definitions 在源码的mavlink库里面可以看到定义了很多.XML文件，这些.XML文件就是需要自己定义的文件，这个文件的写法可以看到：
这个xml文件定义了mavlink消息ID号，消息结构体数据类型，上面是一个系统自定义心跳包的消息自定义。要添加自己的mavlink消息，也要按照这种规范写。主要是消息名字，消息ID和数据结构体，写好之后用前面那个mavlink generator(mavlink 消息生成器)，就可以生成自定义的消息包了。自动生成一些mavlink的消息发送等函数，我们可以看下这个心跳包的函数有哪些：
static inline uint16_t mavlink_msg_heartbeat_pack
static inline uint16_t mavlink_msg_heartbeat_pack_chan
static inline uint16_t mavlink_msg_heartbeat_encode//消息编码函数
static inline uint16_t mavlink_msg_heartbeat_encode_chan
static inline void mavlink_msg_heartbeat_send//消息发送函数
static inline void mavlink_msg_heartbeat_send_struct
static inline void mavlink_msg_heartbeat_send_buf
static inline uint8_t mavlink_msg_heartbeat_get_type
static inline uint8_t mavlink_msg_heartbeat_get_autopilot
static inline uint8_t mavlink_msg_heartbeat_get_base_mode
重要的就是两个消息发送和消息编码函数，在具体的调用函数里面就是调用这个消费发送函数， 把mavlink的消息发送出去，这些都是,mavlink函数自动生成的函数，我们在用的时候调用即可。具体目录为~/src/Firmware/mavlink/include/mavlink/v1.0/common/ mavlink_msg_heartbeat.h
下面看看这个消息生成软件是怎么使用的。
http://dev.px4.io/custom-mavlink-message.html 这个网站可以看到详细的资料，包括mavlink generator的使用。
生成完之后，我们可以看到对应的头文件自动生成。把这些头文件复制到源码里面，就可以调用了。
在源码mavlink的文件夹里面，我们可以找到mavlink_messages.cpp这个cpp的源码。我们可以在这个代码里面看到所有的消息包的实现：
class MavlinkStreamHeartbeat : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamHeartbeat::get_name_static();
}
static const char *get_name_static()
{
return "HEARTBEAT";
}
static uint8_t get_id_static()
{
return MAVLINK_MSG_ID_HEARTBEAT;
}
uint8_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamHeartbeat(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
bool const_rate() {
return true;
}
private:
MavlinkOrbSubscription *_status_sub;
/* do not allow top copying this class */
MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
protected:
explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
{}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
/* always send the heartbeat, independent of the update status of the topics */
if (!_status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
uint8_t base_mode = 0;
uint32_t custom_mode = 0;
uint8_t system_status = 0;
get_mavlink_mode_state(&status, &system_status, &base_mode, &custom_mode);
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
base_mode, custom_mode, system_status);//调用消息发送函数，完成消息发送
}
};
这就是心跳包的mavlink功能实现。这个源码文件里面有很多这样的类，它们都继承了public MavlinkStream这个基类，然后在基类的基础上实现了virtual const char *get_name() const = 0;
virtual uint8_t get_id() = 0;
virtual const char *get_name() const = 0;
virtual bool const_rate() { return false; }
virtual unsigned get_size() = 0;
virtual unsigned get_size_avg() { return get_size(); }
virtual void send(const hrt_abstime t) = 0;
这几个虚函数get_id()是得到消息ID，get_name()是得到消息名字，这些消息ID和消息名字是在前面xml文件里面自己定义好的，通过mavlink generator可以自动生成这些消息ID和消息名字的头文件定义。比如这个心跳包消息
static const char *get_name_static()
{
return "HEARTBEAT";
}
static uint8_t get_id_static()
{
return MAVLINK_MSG_ID_HEARTBEAT;
}
这个return MAVLINK_MSG_ID_HEARTBEAT;就是消息包ID。因为这个 static
uint8_t get_id_static()是无符号8位int型，所以mavlink支持的消息的数量是最多255条消息，其中官方定义已经定义了很多消息包，比如心跳包，姿态包等等。如果我们要自定义消息ID，我们要找到系统没有使用的消息ID，而不能和原有的消息ID重复。
我们可以按照心跳包的消息格式，我们来在mavlink_messages.cpp 写一个自己定义的消息，要在头文件里面添加好我们用mavlink generator生成的mavlink头文件，里面定义了消息ID的编号和这个消息的结构体的函数。
其中还有一个重要的函数mavlink_msg_heartbeat_send 这个函数就是最终的发送mavlink数据包的接口，而这个函数是用mavlink generator(mavlink消息生成器)自动生成好的。封装好了串口的数据发送，我们直接拿来用即可。具体的mavlink_msg_heartbeat_send实现我们可以在生成好的函数里面可以看到，这里不做叙述。
最终我们要追加一个我们自定义的mavlink数据流类到StreamListItem数据流链表里面，
这个链表会在Mavlink_main.cpp里面循环执行链表里面的函数
/* update streams */
MavlinkStream *stream;
LL_FOREACH(_streams, stream)
{
stream->update(t);//循环发送MAVLINK消息包
}
来更新串口mavlink数据流。
注意看到这是个update函数；这个update实际是在调用send函数，这些send函数就是发送串口mavlink数据流。
StreamListItem *streams_list[] = {...new StreamListItem(&MavlinkStreamCaTrajectory::new_instance, &MavlinkStreamCaTrajectory::get_name_static),nullptr};
我们自定义好的MAVLINK消息，就可以在地面站里面显示了，前提是地面站的mavlink包能够解析这个你自定义的数据，一般来讲通过MAVLINK generator生成器生成的mavlink库，和地面站的mavlink库是同一套库，就没问题可以保证解析到。比较偷懒的做法是把自己要发生的消息，通过mavlink原有的包发送出去，地面站解析这个原有的包即可。比如用debug MAVLINk消息包。