Mavlink - 無人機通訊協議

本文轉載自查看原文 2017-05-04 15:23 5119 Mavlink/ 無人機通訊協議/ 地面站

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; }