Mavlink - 无人机通讯协议

时间：2017-05-04 16:12:33 阅读：472 评论：0 收藏：0 [点我收藏+]

http://qgroundcontrol.org/mavlink/start mavlink协议介绍
https://pixhawk.ethz.ch/mavlink/ 消息简介

MAVLink简介

Mavlink协议最早由苏黎世联邦理工学院计算机视觉与几何实验组的 Lorenz Meier于2009年发布，并遵循LGPL开源协议。Mavlink协议是在串口通讯基础上的一种更高层的开源通讯协议，主要应用在微型飞行器（micro aerial vehicle）的通讯上。Mavlink是为小型飞行器和地面站（或者其他飞行器）通讯时常常用到的那些数据制定一种发送和接收的规则并加入了校验（checksum）功能。
协议以消息库的形式定义了参数传输的规则。MavLink协议支持无人固定翼飞行器、无人旋翼飞行器、无人车辆等多种类型的无人机。MAVLink协议是在CAN总线和SAE AS-4 标准的基础上设计形成的。

一 MAVLink所发送的数据结构

如图所示，每个消息帧都是上述的结构，除了灰色外，其他的格子都代表了一个字节的数据。灰色格子里面的数据长度是不固定的。

红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。

第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。

第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。

第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。

第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用）。

第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。

最后两个字节是16位校验位，ckb是高八位，cka是低八位。校验码由crc16算法得到，算法将整个消息（从起始位开始到有效载荷结束，还要额外加上个MAVLINK_CRC_EXTRA字节）进行crc16计算，得出一个16位的校验码。之前提到的每种有效载荷里信息包（由消息包编号来表明是哪种消息包）会对应一个MAVLINK_CRC_EXTRA，这个 MAVLINK_CRC_EXTRA 是由生成mavlink代码的xml文件生成的，加入这个额外的东西是为了当飞行器和地面站使用不同版本的mavlink协议时，双方计算得到的校验码会不同，这样不同版本间的mavlink协议就不会在一起正常工作，避免了由于不同版本间通讯时带来的重大潜在问题。

为了方便叙述，消息包将称作包，包所代表的信息称作消息。上图中的sys将称为sysid，comp将称为compid，msg将称为msgid。
官方的介绍如下图：

上文中已经提到了在mavlink消息帧里最重要的两个东西，一个是msgid；一个是payload，前者是payload中内容的编号，后者则存放了消息。消息有许多种类型，在官网的网页中中以蓝色的“#”加数字的方式来表示消息的编号如 “#0”（这样的表示方法应该是为了方便在网页中查找相应编号消息的定义）。在官网介绍网页里往下拉，大概拉到二分之一的位置处，开始出现“MAVLink Messages”的介绍，往下看是各种消息的数据组成说明。

下面将以几个消息为例，讲解mavlink消息。

先以 #0 消息为例，这个消息叫心跳包（heartbeat）。它一般用来表明发出该消息的设备是活跃的，飞行器和地面站都会发出这个信号（一般以1Hz发送），地面站和飞行器会根据是否及时收到了心跳包来判断是否和飞行器或地面站失去了联系。

1.2

如图所示这是一个APM2.8的控制板输出的一帧心跳包数据，其固件是无人车的固件，可以详细的看到包开始标志，有效载荷长度，消息ID等等。从结构来讲还是比较简单的。这是一个心跳包的一帧数据，因为他的消息ID是00就代表这是一帧心跳包。

心跳包结构如下:

1.3

从图上可以看出，心跳包由6个数据组成

第一个参数是占一个字节的飞行器类型数据（type），这个数据表示了当前发消息的是什么飞行器，比如四旋翼，固定翼等等。type的取值如何与飞行器类型对应，Type表示设备类型在MAV_TYPE有定义可以在https://pixhawk.ethz.ch/mavlink/ 的网页中搜索到，从图1.2可以看到，10是第一位的数据值，那么10在MAV_TYPE代表Ground rover(地面车辆)，这个数据就是从地面车辆的固件里面发出的。同理分析这9个字节所代表的含义可以知道心跳包所代表的含义。在上面网站的文档中可以分析到蓝色(#0,#1,#2)
这样的数据就是代表数据包。在文档的1/2处往后都可以看到，一共有254条消息类型，位于网页开始出的数据枚举中。如下图所示：

这里只是一部分的类型

第一个是通用飞行器，对应的type数值是0；
第二个是固定翼类型，对应的数值是1；
第三个对应的是四旋翼，对应的数值是2.这个飞行器类型，其实对于发心跳包的地面站来说可能没什么意义（不同飞控对该消息的处理方法不同，至少刷了PX4固件的Pixhawk飞控对地面站发来的心跳包里的这个参数并不关心，如无特殊说明，之后所说的Pixhawk飞控都是指刷PX4固件的飞控），对于飞行器端来说代表了当前飞行器的类型，地面站可以根据这个参数来判断飞行器的类型并作出其他的反应。

第二个参数是自驾仪（即通常所说的飞控）类型，比如apm，ppz，Pixhawk等飞控，具体定义查找和之前查找飞行器类型时的方法一样。同样的，对于发送心跳包的飞行器来说代表了自己的飞控类性，对地面站发出的心跳包来说意义不大。

第三个参数是基本模式（base mode），是指飞控现在处在哪个基本模式，对于发心跳包的地面站来说没有意义，对于发送心跳包的飞控来说是有意义的。这个参数要看各个飞控自己的定义方式，mavlink介绍网页并不会给出具体的模式。在Pixhawk中基本模式可以分为使用用户模式（custom mode）还是基本模式（这里有点绕，其实是就是是否使用用户模式）。使用用户模式将在讲下个参数时说明，使用基本模式又会分为自动模式（auto），位置控制模式（posctl）和手动模式（manual）。一般情况下都会使用用户模式，普通用户不用关心这个参数。开发者在使用mavlink修改飞行器模式时需要注意基本模式的设置，具体请看PX4代码，
下载地址https://pixhawk.org/firmware/source_code

另外，Pixhawk的模式和apm的有很大的不同，具体请看官网介绍https://pixhawk.org/users/system_modes 里面还有关于遥控器如何设置模式的教程链接https://pixhawk.org/users/system_modes/mode_switch_config 用QGroundControl地面站（以后简称QGC，下载地址http://qgroundcontrol.org/downloads ）的图形界面来设置飞行模式的功能很鸡肋，建议直接在QGC中读取飞控参数值，并对遥控器的设置参数进行修改，记得改变参数后只是改变了飞控ram参数，要把参数写入到rom中才可以。

第四个参数是用户模式（custom mode），大概说一下Pixhawk的用户模式。以多轴为例。它分为主模式（main mode）和子模式（sub mode），两种模式组合在一起成为最终的模式，主模式分为3种，手动（manual），辅助（assist），自动（auto）。手动模式类似apm的姿态模式。在辅助模式中，又分为高度控制模式（altctl）和位置控制模式（posctl）两个子模式，高度控制模式就类似apm的定高模式，油门对应到飞行器高度控制上。位置模式控制飞行器相对地面的速度，油门和高度控制模式一样，yaw轴控制和手动模式一样。自动模式里又分为3个子模式，任务模式（mission），留待模式（loiter），返航模式（return），任务模式就是执行设定好的航点任务，留待模式就是gps悬停模式，返航模式就是直线返回home点并自动降落。在apm里这个参数貌似是没有用的，注意这个数据占了4个字节，在Pixhawk中，前两个字节（低位）是保留的，没有用，第三个字节是主模式，第四个字节是子模式。普通用户请无视，开发者请注意：官网给出的通过程序设置模式的代码是错误的。如图，最后一行代码有误，应该为：
uint32_t custom_mode = (main_mode<< 16) | (sub_mode << 24);

第五个是系统状态（system status），查定义就好了，其中的standby状态在Pixhawk里就是还没解锁的状态，active状态就是已经解锁，准备起飞的状态。

第六个是mavlink版本（mavlink version），现在是“3”版本。

其余的消息也是类似的结构，各个数据的定义可以查看mavlink官方网页的说明，这些说明一般在网页的前面部分。具体说明以飞控为准，mavlink仅提供基本的定义。

有几个相对特殊和容易混淆的消息再特别说明下：

76消息（command long），该消息是发送长命令，一般是地面站发送给飞控命令用的。该消息组成如下图。目标系统（命令的接收方，就是目标系统编号sysid），目标单元（命令的接收单元，就是目标单元编号compid）。command数据是这条命令的编号，用于区别不同的命令。confirmation数据，笔者还不是很明白，大概是是否需要收到命令后回复确认信号的意思。接下去有七个参数，这些参数是执行这条命令所需要告诉飞控的，许多命令都用不到七个参数，多余的参数清0就可以了。

Pixhawk支持的命令有许多种（但不是所有mavlink命令都支持）。要看mavlink提供了哪些命令请在介绍mavlink的官网查询mav_cmd，在网页的中上部分。比如：第176号命令 MAV_CMD_DO_SET_MODE。这条命令用于改变飞行器的飞行模式，第一个参数就是设置飞控的base_mode，第二个是设置custom_mode。想要通过这条命令正确设置pixhawk的模式需要查看PX4代码，mavlink对参数的描述不够具体。

现在应该对介绍mavlink官网的布局有所了解了吧。网页前面主要讲了各类数据的取值和含义，比如飞控类型（mav_autopilot）,飞行器类型（mav_type）等，其中mav_cmd是比较特殊和重要的一种数据。网页的后半部分主要讲了mavlink消息的种类和数据组成，这里会用到各种数据，具体数据定义的可以回到前半部分去找。但是mavlink是个通用的通讯协议，不同的飞控支对mavlink支持方式不一样，一般都只支持一部分mavlink消息，还会自己扩展一些mavlink协议所没有定义的消息（pixhawk和apm都是如此），具体都以飞控代码为准。

二地面站和飞控的通讯流程

由于没看过地面站的代码，所以很可能有误，还望发评论指正！一般飞控在连接上地面站后都会主动向地面站发送心跳包，飞行器姿态，系统状态，遥控器信号等组成的数据流。各个数据都会以一定的频率发送，比如心跳包一般是1Hz，姿态信息会快些，pixhawk用数传连接QGC时的姿态数据发送频率在7-8Hz左右。一般地面站会在刚连接上飞控时发送命令，请求飞控传回所有参数（QGC就是这样），飞控根据自己的情况判断是否接受地面站的请求，并根据不同的命令执行相应的操作（有些命令需要飞控回复地面站确认信号）。之后地面站根据用户的操作会发送相应的mavlink消息给飞控，比如设置航点，改写飞控参数等。据说数传是半双工的（在同一时刻只能选择发送或者选择接受数据，不能同时收发数据），地面站和飞控之间如何避免数据冲突（即双方同时向对方发送消息）的机制笔者并不清楚，希望能抛砖引玉。

可以看到，里面有多个文件夹和几个头文件。pixhawk，ardupilotmega（apm），matrixpilot这类的文件夹里都是各个飞控自己定义的mavlink消息类型
原始的mavlink消息放在common文件夹里面（大部分消息都在common文件夹中）。checksum.h中存放的是计算校验码的代码。
mavlink_helper.h里面是将各个消息包补充完整（调用checksum.h中的函数计算校验码并补上消息帧的头，比如sysid和compid等）成为mavlink消息帧再发送。最主要的功能集中在这两个文件夹中。
mavlink_conversions.h里是dcm，欧拉角，四元数三种姿态表示方法之间的转换代码。

下面以发送心跳包（heartbeat）为例，说明下如何使用mavlink头文件来发送心跳包。首先打开common文件夹中的 mavlink_msg_heartbeat.h 头文件。这个头文件可以分为两部分，一部分用来打包、发送heartbeat消息，另一部分用来接收到heartbeat消息时解码消息。heartbeat.h定义了heartbeat消息对应的数据类型：

typedef struct __mavlink_heartbeat_t {
    uint32_t custom_mode; ///< A bitfield for use for autopilot-specific flags.
    uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types,     defined in MAV_TYPE ENUM)
    uint8_t autopilot; ///< Autopilot type / class. defined in MAV_AUTOPILOT ENUM
    uint8_t base_mode; ///< System mode bitfield, see MAV_MODE_FLAG ENUM in     mavlink/include/mavlink_types.h
    uint8_t system_status; ///< System status flag, see MAV_STATE ENUM
    uint8_t mavlink_version; ///< MAVLink version, not writable by user, gets added by     protocol because of magic data type: uint8_t_mavlink_version
} mavlink_heartbeat_t;

如果mavlink的发送方式可以使用（串口发送，函数接口也兼容），则可以调用

   static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)

其中的chan是channel的缩写，用于选择发送的串口或者usb口。type就是飞行器类型，其余参数不明的可以看看本博客的第一篇文章。
该函数功能是将传入的各个参数按照对应的格式放到heartbeat消息包中（即打包）
这个函数内部有一句预处理：

#if MAVLINK_CRC_EXTRA

是说是否使用额外的crc校验字符（默认使用），详情请看第一篇博客中对于两个校验字节的说明。
函数中会调用函数

_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);

MAVLINK_MSG_ID_HEARTBEAT//这个是心跳包消息对应的编号 这里=0

MAVLINK_MSG_ID_HEARTBEAT_LEN//这个是心跳包的长度 注意这个长度仅仅是payload的长度，不包括帧的头尾。

MAVLINK_MSG_ID_HEARTBEAT_CRC//这个是heartbeat消息对应的额外的crc校验码 这里=50

这个函数位于mavlink_helper.h中，用于更新消息帧的编号（seq 每发送一帧加1）并将消息帧的头和计算校验码，使得成为完整的一个mavlink消息帧。最后调用串口发送函数进行消息帧的发送。

如果只是想将对应的心跳包参数按照心跳包的格式存放好，则可以只调用

static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)

将参数打包为heartbeat消息帧，待之后使用。
解码消息帧时可以调用mavlink_helper.h中的

MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)

它会将收到的字符一个个进行解码，会检验收到的校验码是否正确；有效载荷的长度小于最大长度并且和该消息的长度一致。如果一切顺利，将会得到解码到的消息，放在解码得到的消息帧类型中

typedef struct __mavlink_message {
    uint16_t checksum; ///< sent at end of packet
    uint8_t magic; ///< protocol magic marker
    uint8_t len; ///< Length of payload
    uint8_t seq; ///< Sequence of packet
    uint8_t sysid; ///< ID of message sender system/aircraft
    uint8_t compid; ///< ID of the message sender component
    uint8_t msgid; ///< ID of message in payload
    uint64_t     payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;

其中的magic是一帧的起始标志（FE=254），就是mavlink_stx的值。

其余的mavlink消息也是类似的，旧的mavlink代码中有些类型的消息类型可能会找不到，使用时要注意接受和发送方使用的mavlink版本是否兼容。common文件夹中的common.h里面包含了要用到的数据类型和所有消息的头文件，使用时直接包含进来即可。

消息包封装过程

以上各区域信息存在关联，当使用MavLink协议提供的方法封装消息包时，会根据所使用的MSG获取到该类别MSG消息的LEN信息，同时软件（地面站或飞行控制软件）会根据自身状态信息填写SYS、COMP。信息填写完毕生成数据包时，封装方法会自动添加STX，并在上一次发送消息包所使用的SEQ上加1作为本次发送的SEQ写入，当SEQ超过255时，会回到0并重新开始计数。CKA、CKB会在PAYLOAD信息写入后、封装完成之前，根据CRC[Fe1] （CyclicRedundancy Check）循环冗余校验码算法计算得出并自动写入包内。
也就是说，设定SYS和COMP并且正确调用MavLink所提供方法后，整个消息包的生成过程中仅有MSG和PAYLOAD两项内容需要用户关心，消息包封装过程如活动图所示。

消息包示例
本文开始提到MavLink使用消息库的形式定义传输规则，用户可以在在源码中查阅消息库的内容，此处使用Java语言下的消息库作为实例，以便更清晰地展示包结构（MavLink源码自带了多语言的生成器，可从源码中的xml文件转换为对应C，C++，Java等语言的MavLink协议包）。以下表格中，SEQ为计算得出，数值不固定，故用X代替，SYS，COMP两项为笔者使用的Mission Planner地面站设定的系统ID和组件ID，MSG项0代表HEARTBEAT消息的ID，PAYLOAD内存储详细信息，最后的CKA CKB为封包后计算得出，以Y代替。
msg_heartbeat：最基本的心跳信号包，周期性发送，用于确认地面站与无人机之间的连接是否有效。

msg_request_data_stream：数据流请求包，地面站使用该消息包向飞行控制软件提交数据流申请，飞行控制软件收到该消息后将按照设定的参数周期性返回消息包。

大概知道了mavlink协议之后呢，我们就可以用MAVLINK结合APM2.6来实现一个APM的地面站。参考Mission Planner的地面站，Mission Planner是采用C#言语，基于C#的易用性，我们也采用C#语言。不过在编写一个简版的地面站以前。我们可以参考Mission Planner的构架，我们可以安装好VS2013，编译一下Mission Planner源码，如果有能力，可以修改Mission Planner源码。MP地面站的编译方法已经在另一篇文章中详细讲述过，编译完了之后有个SimpleExample的简单代码例程在MP的源码中，我们可以分析下这简单的代码，这个简单的代码是引用了MAVLINK的库，具体位置在MissionPlanner-master\ExtLibs\Mavlink中，如果我们要用只要在新建的工程里面引用即可，知道MAVLINK是怎么解析的。

消息解析

分析过消息包的结构后，继续向消息包的内部探索，开始分析负载信息PAYLOAD
在消息库中，每条消息都作为一个类存在（Java版本），类中的注释文本详细地注明了每个成员变量代表的含义。这些成员变量不仅包括STX、SEQ这些包的描述信息，还包括封装入`PAYLOAD的各个参数。在消息类中，还包含了pack()打包方法和unpack() 解包方法，为地面站和飞行控制软件的开发、应用提供了接口。
负载信息PAYLOAD内，大部分数据以Byte类型存储，同时也存在float型、short型等类型。需要注意的是，在Java中byte类型有符号位，能够覆盖-128—127范围，而C中byte为无符号位，覆盖范围为0—255。这一类差异在MavLink提供的方法中已经得到了妥善处理，使用MavLink协议提供的方法封装信息时无需担心，但如果是自己写封包解包方法，需要注意解析后读取参数值的类型转换问题。
负载信息的长度和格式并不统一，这是由于不同类型的消息包所需要传递的参数不一致而形成的。通过自己编写的“伪飞控”向地面站发送消息并接收，可以从Mission Planner地面站获取一系列消息包。查询各消息包内PAYLOAD含义，翻译后得到如下文本。
`msg_request_data_stream：该消息的作用在上一章里已讲，现在读取其负载信息。

req_message_rate：该类型消息请求间隔，用于控制飞控板向地面站发送指定类型消息的频率。
target_system，target_component：目标系统ID，笔者使用的Pixhawk飞控板这两项参数为1，1。
req_stream_id：请求数据流ID。非常重要的属性！该参数将直接影响飞控板返回数据流内信息包的类型，数据流ID为11时可以周期性收到VFR_HUD消息包和GLOBAL_POSITION_INT消息包，分别储存了平视显示器所需数据和GPS信息。
start_stop：数据流开始发送或停止发送标识。设为1则代表开始发送该数据流。

其他消息包内容由控制台打印，挑选部分展示。

MAVLINK_MSG_ID_ATTITUDE -time_boot_ms:16777195 roll:0.021675825 pitch:0.012977336 yaw:1.3565465rollspeed:-8.172542E-5 pitchspeed:-3.5201595E-4 yawspeed:3.577727E-4
Attitude状态报告，包括滚转角、偏航角、俯仰角（及其速度）等信息。

MAVLINK_MSG_ID_VFR_HUD -airspeed:0.0 groundspeed:0.0 alt:-0.07 climb:0.02 heading:77 throttle:0
共有 64 字节
平视显示器数据 Head Up Display

地面站接收到数据：
MAVLINK_MSG_ID_GLOBAL_POSITION_INT -time_boot_ms:16777215 lat:0 lon:0 alt:0 relative_alt:-70 vx:0 vy:0 vz:0hdg:7772
共有 36 字节
GPS定位信息


地面站接收到数据：
MAVLINK_MSG_ID_SYS_STATUS -onboard_control_sensors_present:16776207onboard_control_sensors_enabled:16751631onboard_control_sensors_health:16776207 load:130 voltage_battery:0current_battery:-1 drop_rate_comm:0 errors_comm:0 errors_count1:0errors_count2:0 errors_count3:0 errors_count4:0 battery_remaining:-1
常规系统状态信息
onboard_control_sensors_present：以位掩码表示控制器及传感器的存在状态，16776207（十进制）= 111111111111110000001111（二进制）
onboard_control_sensors_enabled：以位掩码表示控制器及传感器的启用状态，16751631（十进制）= 111111111001110000001111（二进制)
onboard_control_sensors_health：以位掩码表示控制器及传感器处于可用状态还是存在错误。转换为二进制同上。
以上掩码信息中，第一位表示gyro陀螺仪，第二位表示accelerometer加速度计，第六位表示GPS……详情见MAV_SYS_STATUS文件。
Load： Maximum usage in percent of the mainloop time,主循环内时间的最大使用比例，1000表示100%，该值应保持小于1000。
voltage_battery:电池电压，单位毫伏特。
current_battery：当前电池（电流），单位毫安。-1表示飞控未测量。
drop_rate_comm：通信丢失百分比，1000表示100%。
errors_comm：通信错误 (UART, I2C, SPI, CAN)，丢包。
Errors_countX:Autopilot-specific errors,飞控特定错误，未知含义。
battery_remaining:剩余电量，1表示1%，-1：autopilot estimate the remainingbattery，飞控估计电量。


MAVLINK_MSG_ID_POWER_STATUS -Vcc:4962 Vservo:21 flags:4
Vcc：5V rail voltage in millivolts，5V轨道电压，单位为毫伏。
Vservo：servo rail voltage in millivolts，伺服（电机？）轨道电压，单位为毫伏。
Flags：power supply status flags，供电状态标识，4表示MAV_POWER_STATUS_USB_CONNECTED，USB供电。详见MAV_POWER_STATUS。

MAVLINK_MSG_ID_MEMINFO - brkval:0freemem:52240
位于Ardupilotmega包内，记录内存信息
Brkval：heap top，堆顶
Freemem：空闲内存大小，单位为字节。

MAVLINK_MSG_ID_MISSION_CURRENT -seq:0
声明当前活动任务项的序列号

MAVLINK_MSG_ID_GPS_RAW_INT -time_usec:0 lat:0 lon:0 alt:0 eph:0 epv:65535 vel:0 cog:0 fix_type:0satellites_visible:0
The global position, as returned by the Global PositioningSystem (GPS). This is NOT the global position estimate of the system, butrather a RAW sensor value. See message GLOBAL_POSITION for the global positionestimate. Coordinate frame is right-handed, Z-axis up (GPS frame)
全球定位，并非系统估计位置，而是RAW传感器值。（Raw Sensor 原始传感器？）——右手坐标系，Z轴向上。
time_usec：时间戳，单位为microseconds微秒。
Lat：Latitude (WGS84), in degrees * 1E7，纬度，单位为度数*10的7次方。
Lon：Longitude (WGS84), in degrees * 1E7，经度，单位为度数*10的7次方。
alt：Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up).Note that virtually all GPS modules provide the AMSL altitude     in addition to the WGS84 altitude.高度，单位为km，向上为正。注意所有GPS模块除了WGS84高度以外，均提供AMSL（平均海平面以上）高  度。
Eph：GPS HDOP （horizontal dilution of position） in cm (m*100). If unknown, set to: UINT16_MAX，GPS水平精度因子，单位为厘米。
Epv： GPS VDOP vertical dilution of position in cm (m*100). Ifunknown, set to: UINT16_MAX，GPS垂直精度因子，单位为厘米。
Vel： GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX，全球定位系统地速度，单位为百米每秒。
Cog：Course over ground (NOT heading, but direction of movement) indegrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
 实际航迹向，单位为百分之一度。
fix_type：0-1: no fix, 2: 2D fix, 3: 3D fix, 4: DGPS, 5: RTK.GPS修正类型。
satellites_visible：卫星可见数，未知则填写255。

MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT- nav_roll:0.0 nav_pitch:0.0 alt_error:0.14554046 aspd_error:0.0xtrack_error:0.0 nav_bearing:77 target_bearing:0 wp_dist:0
Outputs of the APM navigation controller. The primary use ofthis message is to check the response and signs of the controller before actualflight and to assist with tuning controller parameters.
APM导航控制器的输出，该信息主要功能为检查实飞前控制器的回复和信号，辅助调整控制参数。
nav_roll：当前所需的滚转角
……
alt_error：高度误差
aspd_error：当前空速误差 m/s
xtrack_erro：Current crosstrack error on x-y plane in meters.当前x-y平面横向轨迹误差  单位m
nav_bearing：Current desired heading in degrees.当前任务/目标方位单位度
target_bearing：Bearing to current MISSION/target in degrees.当前任务/目标方位单位度
wp_dist：Distance to active MISSION in meters就，至当前任务点的距离，单位为m
共有 147 字节

地面站接收到数据：
MAVLINK_MSG_ID_RAW_IMU -time_usec:4287561674 xacc:11 yacc:-9 zacc:-995 xgyro:0 ygyro:0 zgyro:0 xmag:25ymag:-133 zmag:325
The RAW IMU readings for the usual 9DOF sensor setup. Thismessage should always contain the true raw values without any scaling to allowdata capture and system debugging.
RAW IMU惯性测量单元此消息始终只包含原始信息
time_usec：时间戳
Xacc：X acceleration (raw) X轴向加速度
Yacc：Y acceleration (raw) Y轴向加速度
Zacc：Z acceleration (raw) Z轴向加速度
Xgyro： Angular speed around X axis (raw) X轴旋转角速度
Ygyro： Angular speed around Y axis (raw) Y轴旋转角速度
Zgyro： Angular speed around Z axis (raw) Z轴旋转角速度
Xmag：X Magnetic field (raw) X磁场
Ymag：Y Magnetic field (raw) Y磁场
Zmag：Z Magnetic field (raw) Z磁场

MAVLINK_MSG_ID_SCALED_IMU2 -time_boot_ms:16777099 xacc:21 yacc:-6 zacc:-986 xgyro:-2 ygyro:3 zgyro:-6xmag:0 ymag:0 zmag:0
The RAW IMU readings for secondary 9DOF sensor setup. Thismessage should contain the scaled values to the described units
各项参数含义同上

MAVLINK_MSG_ID_SCALED_PRESSURE -time_boot_ms:16777099 press_abs:959.9057 press_diff:-0.0509375 temperature:3720
The pressure readings for the typical setup of one absolute anddifferential pressure sensor. The units are as specified in each field.绝对压强和压差传感器的读数。
time_boot_ms：时间戳
press_abs：Absolute pressure (hectopascal)，绝对压强，单位百帕斯卡
press_diff：Differential pressure 1 (hectopascal)，压差，单位百帕斯卡
temperature：Temperature measurement (0.01 degrees celsius) 温度，单位为0.01摄氏温度

MAVLINK_MSG_ID_SERVO_OUTPUT_RAW -time_usec:4287561666 servo1_raw:1016 servo2_raw:1016 servo3_raw:1016servo4_raw:1016 servo5_raw:0 servo6_raw:0 servo7_raw:0 servo8_raw:0 port:0
The RAW values of the servo outputs (for RC input from theremote, use the RC_CHANNELS messages). The standard PPM modulation is asfollows: 1000 microseconds: 0%, 2000 microseconds: 100%. 伺服系统输出原始值，（远程RC输入使用RC_CHANNELS消息）标准PPM调制如下：1000微秒：0%，2000微秒100%。
time_usec：时间戳
servo1_raw：Servo output 1 value, in microseconds，伺服输出1的值，单位微秒。
……
Port：Servo output port (set of 8 outputs = 1 port). MostMAVs will just use one, but this allows to encode more than 8 servos.
      伺服输出端口，多数MAV只使用一个端口，最多可支持8伺服的编码。

MAVLINK_MSG_ID_RC_CHANNELS_RAW -time_boot_ms:16777099 chan1_raw:0 chan2_raw:0 chan3_raw:0 chan4_raw:0chan5_raw:0 chan6_raw:0 chan7_raw:0 chan8_raw:0 port:0 rssi:0
The RAW values of the RC channels received. The standard PPMmodulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%.Individual receivers/transmitters might violate this specification. RC通道接收的原始值，标准PPM调制如下：1000微秒：0%，2000微秒100%。不同接收器或转换器可能违反该规范。
time_usec：时间戳
chan1_raw:0 RC channel 1 value, in microseconds. A value ofUINT16_MAX implies the channel is unused.遥控通道1，单位为微秒。设为无符号位16位整数最大值表示该通道未使用。
……
port:0 Servo output port (set of 8 outputs = 1 port). Most MAVswill just use one, but this allows for more than 8 servos.
rssi:0 Receive signal strength indicator, 0: 0%, 100: 100%, 255:invalid/unknown.接收信号强度指示器，最小为0，最大为100

MAVLINK_MSG_ID_AHRS -omegaIx:-3.326245E-5 omegaIy:-1.11720045E-4 omegaIz:-3.0743552E-4accel_weight:0.0 renorm_val:0.0 error_rp:0.0013528633 error_yaw:0.0010219128
Status of DCM attitude estimator
omegaIx: X gyro drift estimate rad/s  X轴方向陀螺仪漂移量
omegaIy: Y gyro drift estimate rad/s
omegaIz: Z gyro drift estimate rad/s
accel_weight: average accel_weight.  平均加速权重（？）
renorm_val: average renormalisation value   平均重正化值（？）
error_rp: average error_roll_pitch value  ？
error_yaw: average error_yaw value  ？

MAVLINK_MSG_ID_HWSTATUS - Vcc:4962I2Cerr:0
Status of key hardware  关键硬件状态
Vcc：board voltage (mV)  飞控电压  毫伏
I2Cerr：I2C error count I2C错误计数

MAVLINK_MSG_ID_SYSTEM_TIME -time_unix_usec:0 time_boot_ms:16777100
The system time is the time of the master clock, typically thecomputer clock of the main onboard computer.
系统时间
time_unix_usec：Timestamp of the master clock in microseconds sinceUNIX epoch. 主时钟UNIX时间戳，1970-1-1
time_boot_ms：Timestamp of the component clock since boot time inmilliseconds. 自启动开始计时，时钟组件时间戳单位毫秒

MAVLINK解析示例

主代码如下：

namespace SimpleExample {
    public partial class simpleexample : Form  {
    MAVLink.MavlinkParse mavlink = new MAVLink.MavlinkParse();//MAVLINK解析包函数，通过这个函数从串口得到一帧的数据
    bool armed = false;
    public simpleexample() {
        InitializeComponent();
    }
    private void but_connect_Click(object sender, EventArgs e) {
        // if the port is open close it
        if (serialPort1.IsOpen) {
        serialPort1.Close();
        return;
    }
    // set the comport options
    serialPort1.PortName = CMB_comport.Text;//打开串口
    serialPort1.BaudRate = int.Parse(cmb_baudrate.Text);
    // open the comport
    serialPort1.Open();
    // set timeout to 2 seconds
    serialPort1.ReadTimeout = 2000;
    // request streams - asume target is at 1,1
    mavlink.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.REQUEST_DATA_STREAM,
        new MAVLink.mavlink_request_data_stream_t() {
        req_message_rate = 2,
        req_stream_id = (byte)MAVLink.MAV_DATA_STREAM.ALL
        start_stop = 1,
        target_component = 1,
        target_system = 1
    });//设置mavlink数据缓冲区格式
    while (serialPort1.IsOpen) {
        try  {
            // try read a hb packet from the compo
            var hb = readsomedata<MAVLink.mavlink_heartbeat_t>();//读取一帧心跳数据包
            var att = readsomedata<MAVLink.mavlink_attitude_t>();//读取一帧姿态包
            Console.WriteLine(att.pitch*57.2958 + " " + att.roll*57.2958);//写入命令行
        }  catch  {
        }
      System.Threading.Thread.Sleep(1);
      Application.DoEvents();
    }
}
T readsomedata<T>(int timeout = 2000)//读取数据函数 {
    DateTime deadline = DateTime.Now.AddMilliseconds(timeout);
// read the current buffered bytes
    while (DateTime.Now < deadline) {
         var packet = mavlink.ReadPacketObj(serialPort1.BaseStream);//读出一帧数据
         if (packet == null)
               continue;
          Console.WriteLine(packet);//打印在控制台
         if (packet.GetType() == typeof(T)) {
              return (T)packet;//从一帧MAVLINK数据中返回T类型的数据，T类型是在调用处指定的可以是心跳包，或者姿态包等等。
         }
     }
    throw new Exception("No packet match found");
}
MAVLink.MavlinkParse mavlink 最重要的是这个类，这个类是解析一帧数据包用的，我们来分析下
public partial class MAVLink {
    public static void ReadWithTimeout
    public byte[] ReadPacket
    public object ReadPacketObj//读取一包数据
    public byte[] GenerateMAVLinkPacket//生成一个MAVLINK包，可以同个这个函数来生成读取数据包或者写命令数据包，在解析数据和读取MAVLINK数据之前都要先调用这个函数，生成一个空包，以便放入数据或者写入数据，相当于开辟一个MAVLINK数据缓冲区。
}

以上的程序运行如下：

图-读取MAVLINK数据包
可以看到在
var hb = readsomedata<MAVLink.mavlink_heartbeat_t>();
var att = readsomedata<MAVLink.mavlink_attitude_t>();
在mavlink_heartbeat_t这个类中有心跳包的成员变量六个。
在MAVLink.mavlink_attitude_t这类中有姿态成员变量。我们可以在VS里面清楚的看到。所以如果我们想读取心跳包和姿态包，就可以使用上面的方法来获取心跳和姿态，很方便。通过上面的代码，我们就可以从APM的串口MAVLINK数据流中解析出想要的数据。我们想读取其他数据，在readsomedata中写入不同的形参即可，具体还有其他什么形参可以在VS里面查看。如果我们要更复杂的界面，做出专业的地面站，就去布局控件即可，不过还是比较复杂的，可以参考MP地面站。

UML图

绘制dependencyLibs的UML图。该包主要提供了MavLink的所有类型的封包类和解析类。

 /**
    * Encode this packet for transmission.
    *
    * @return Array with bytes to be transmitted
    */
    public byte[] encodePacket() {
        byte[] buffer = new byte[6 + len + 2];

        int i = 0;
        buffer[i++] = (byte) MAVLINK_STX;
        buffer[i++] = (byte) len;
        buffer[i++] = (byte) seq;
        buffer[i++] = (byte) sysid;
        buffer[i++] = (byte) compid;
        buffer[i++] = (byte) msgid;

        final int payloadSize = payload.size();
        for (int j = 0; j < payloadSize; j++) {
            buffer[i++] = payload.payload.get(j);
        }

        generateCRC();
        buffer[i++] = (byte) (crc.getLSB());
        buffer[i++] = (byte) (crc.getMSB());
        return buffer;
    }

解包的核心部分在Parser类中：

/**
     * This is a convenience function which handles the complete MAVLink
     * parsing. the function will parse one byte at a time and return the
     * complete packet once it could be successfully decoded. Checksum and other
     * failures will be silently ignored.
     * 
     * @param c
     *            The char to parse
     */
    public MAVLinkPacket mavlink_parse_char(int c) {
        msg_received = false;

        switch (state) {
        case MAVLINK_PARSE_STATE_UNINIT:
        case MAVLINK_PARSE_STATE_IDLE:

            if (c == MAVLinkPacket.MAVLINK_STX) {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_STX:
            if (msg_received) {
                msg_received = false;
                state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
            } else {
                m = new MAVLinkPacket(c);
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_LENGTH;
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_LENGTH:
            m.seq = c;
            state = MAV_states.MAVLINK_PARSE_STATE_GOT_SEQ;
            break;

        case MAVLINK_PARSE_STATE_GOT_SEQ:
            m.sysid = c;
            state = MAV_states.MAVLINK_PARSE_STATE_GOT_SYSID;
            break;

        case MAVLINK_PARSE_STATE_GOT_SYSID:
            m.compid = c;
            state = MAV_states.MAVLINK_PARSE_STATE_GOT_COMPID;
            break;

        case MAVLINK_PARSE_STATE_GOT_COMPID:
            m.msgid = c;
            if (m.len == 0) {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
            } else {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_MSGID;
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_MSGID:
            m.payload.add((byte) c);
            if (m.payloadIsFilled()) {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_PAYLOAD;
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
            m.generateCRC();
            // Check first checksum byte
            if (c != m.crc.getLSB()) {
                msg_received = false;
                state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
                if (c == MAVLinkPacket.MAVLINK_STX) {
                    state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
                    m.crc.start_checksum();
                }
                stats.crcError();
            } else {
                state = MAV_states.MAVLINK_PARSE_STATE_GOT_CRC1;
            }
            break;

        case MAVLINK_PARSE_STATE_GOT_CRC1:
            // Check second checksum byte
            if (c != m.crc.getMSB()) {
                msg_received = false;
                state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
                if (c == MAVLinkPacket.MAVLINK_STX) {
                    state = MAV_states.MAVLINK_PARSE_STATE_GOT_STX;
                    m.crc.start_checksum();
                }
                stats.crcError();
            } else { // Successfully received the message
                stats.newPacket(m);
                msg_received = true;
                state = MAV_states.MAVLINK_PARSE_STATE_IDLE;
            }

            break;

        }
        if (msg_received) {
            return m;
        } else {
            return null;
        }
}

这个类的使用是逐个直接解析，解析完毕后返回完整的包，例如，对字节数组packet，解析过程如下：

for(int i = 0; i < packet.length - 1; i++){
    parser.mavlink_parse_char(packet[i] & 0xFF);//每次解析1位
}
MAVLinkPacket m = parser.mavlink_parse_char(packet[packet.length - 1] & 0xFF);//最后1位即可返回

MavLink包测试

dependencyLibs提供了测试实例。以msg_altitude为例，判断生成的包和解析的包是否相同，即可判断该类是否正确。

/**
* The current system altitude.
*/
public class msg_altitude_test{

public static final int MAVLINK_MSG_ID_ALTITUDE = 141;
public static final int MAVLINK_MSG_LENGTH = 24;
private static final long serialVersionUID = MAVLINK_MSG_ID_ALTITUDE;

private Parser parser = new Parser();//1位解析类

public CRC generateCRC(byte[] packet){
    CRC crc = new CRC();
    for (int i = 1; i < packet.length - 2; i++) {
        crc.update_checksum(packet[i] & 0xFF);
    }
    crc.finish_checksum(MAVLINK_MSG_ID_ALTITUDE);
    return crc;
}

public byte[] generateTestPacket(){
    ByteBuffer payload = ByteBuffer.allocate(6 + MAVLINK_MSG_LENGTH + 2);
    payload.put((byte)MAVLinkPacket.MAVLINK_STX); //stx
    payload.put((byte)MAVLINK_MSG_LENGTH); //len
    payload.put((byte)0); //seq
    payload.put((byte)255); //sysid
    payload.put((byte)190); //comp id
    payload.put((byte)MAVLINK_MSG_ID_ALTITUDE); //msg id
    payload.putFloat((float)17.0); //altitude_monotonic
    payload.putFloat((float)45.0); //altitude_amsl
    payload.putFloat((float)73.0); //altitude_local
    payload.putFloat((float)101.0); //altitude_relative
    payload.putFloat((float)129.0); //altitude_terrain
    payload.putFloat((float)157.0); //bottom_clearance

    CRC crc = generateCRC(payload.array());
    payload.put((byte)crc.getLSB());
    payload.put((byte)crc.getMSB());
    return payload.array();
}

@Test
public void test(){
    byte[] packet = generateTestPacket();
    for(int i = 0; i < packet.length - 1; i++){
        parser.mavlink_parse_char(packet[i] & 0xFF);//每次解析1位
    }
    MAVLinkPacket m = parser.mavlink_parse_char(packet[packet.length - 1] & 0xFF);//最后1位即可返回
    byte[] processedPacket = m.encodePacket();//解析
    assertArrayEquals("msg_altitude", processedPacket, packet);
    }
}

Mavlink 协议硬解析主要代码

int MAVLinkProtocol::ParseMsg(BYTE arMsgBuf[], MSGVALUE *pMavMsg, CString &strMsgText)
{
    // Function    : 
    // Parameters  : arMsgBuf - 为完整的 mavlink msg 缓冲区, 从 0xFE 开始到 最后一个校验字节(CKB)
    // Return value: 
    // Remark      : 

    int  i, nOfst, nLenMaxPayload;
    BYTE nMsgID;
    char szTmpName[LEN_MSGNM];

    char szStatusTxt[MAVLINK_MSG_ID_STATUSTEXT_LEN];
    char szValueID[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
    CString strTmp, strUnicodeTmp;


    i = 0;
    nOfst = 0;

    if(arMsgBuf[0] != 0xfe)
        return -1;

    nMsgID = arMsgBuf[5];
    ASSERT(nMsgID < 256);

    nLenMaxPayload = arMsgBuf[1];
    ASSERT(nLenMaxPayload > 0 && nLenMaxPayload < MAVLINK_MAX_PAYLOAD_LEN);


    // 获取 message 字段个数
    pMavMsg ->nFieldCnts = g_arMsgInfo[nMsgID].num_fields;

    // 获取 message 名称
    memset(szTmpName, 0x00, sizeof(szTmpName));
    if(strlen(g_arMsgInfo[nMsgID].name) < LEN_MSGNM)
        strcpy(szTmpName,  g_arMsgInfo[nMsgID].name);
    else
        strncpy(szTmpName, g_arMsgInfo[nMsgID].name, LEN_MSGNM - 1);

    strUnicodeTmp = AnsiStr2Unicode(szTmpName);
    if(strUnicodeTmp.GetLength() > LEN_MSGNM - 1)
        strUnicodeTmp = strUnicodeTmp.Left(LEN_MSGNM - 1);
    _tcscpy(pMavMsg ->szMsgName, strUnicodeTmp);



    // 根据 g_arMsgInfo[] 获取并处理各字段信息
    for(i = 0; i < pMavMsg ->nFieldCnts; i++)
    {
        strUnicodeTmp = AnsiStr2Unicode((char *) g_arMsgInfo[nMsgID].fields[i].name);

        if(strUnicodeTmp.GetLength() > LEN_FIELDNM - 1)
            strUnicodeTmp = strUnicodeTmp.Left(LEN_FIELDNM - 1);

        _tcscpy(pMavMsg ->arField[i].szFieldNm, strUnicodeTmp);                                // Field Name
        pMavMsg ->arField[i].nFieldTyp = g_arMsgInfo[nMsgID].fields[i].type;                // Field Type

        nOfst = g_arMsgInfo[nMsgID].fields[i].wire_offset;


        // 特殊消息的处理
        if(nMsgID == MAVLINK_MSG_ID_STATUSTEXT && i == 1)            
        {
            memset(szStatusTxt, 0x00, sizeof(szStatusTxt));
            memcpy(szStatusTxt, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 1), MAVLINK_MSG_ID_STATUSTEXT_LEN);        // 1 为 字符串位置相对载荷开始位置的偏移
            strMsgText = AnsiStr2Unicode(szStatusTxt);
        }
        else if(nMsgID == MAVLINK_MSG_ID_PARAM_VALUE && i == 3)
        {
            memset(szValueID, 0x00, sizeof(szValueID));
            memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 8), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
            strMsgText = AnsiStr2Unicode(szValueID);
        }
        else if(nMsgID == MAVLINK_MSG_ID_PARAM_SET && i == 3)
        {
            memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 6), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
            strMsgText = AnsiStr2Unicode(szValueID);
        }
        else
        {
            // 一般 mavlink msg 的处理

            // 获取每个数值所存储的缓冲区
            if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_CHAR || 
               g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT8_T || 
               g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT8_T)
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                ASSERT(nOfst < nLenMaxPayload);
            }
            else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT16_T || 
                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT16_T)
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];

                ASSERT(nOfst + 1 < nLenMaxPayload);
            }
            else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT32_T || 
                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT32_T)
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];

                ASSERT(nOfst + 3 < nLenMaxPayload);
            }
            else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT64_T ||
                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT64_T)
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];
                pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];
                pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];
                pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];
                pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];

                ASSERT(nOfst + 7 < nLenMaxPayload);
            }
            else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_FLOAT)            // 4 byte
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];

                ASSERT(nOfst + 3 < nLenMaxPayload);
            }
            else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_DOUBLE)        // 8 byte
            {
                pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];
                pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];
                pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];
                pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];
                pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];    

                ASSERT(nOfst + 7 < nLenMaxPayload);
            }
            else 
            {
                TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - Unexpect field type: "));
                TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - g_arMsgInfo[nMsgID].fields[i].type = %d"), g_arMsgInfo[nMsgID].fields[i].type);
            }
        }
    }

    return nMsgID;
}