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文件就是需要自己定义的文件，这个文件的写法可以看到：

<message id="0" name="HEARTBEAT">

<description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>

<field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>

<field type="uint8_t" name="autopilot">Autopilot type / class. defined in MAV_CLASS ENUM</field>

<field type="uint8_t" name="base_mode">System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h</field>

<field type="uint32_t" name="custom_mode">Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific.</field>

<field type="uint8_t" name="system_status">System status flag, see MAV_STATUS ENUM</field>

<field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>

</message>

 

这个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消息包。