現(xiàn)MAVLink協(xié)議解析與封包：幀結(jié)構(gòu)、CRC校驗(yàn)與工程實(shí)踐)
搞過無人機(jī)地面站、數(shù)傳鏈路或者飛控二次開發(fā)的朋友對(duì)MAVLink這個(gè)協(xié)議應(yīng)該都不陌生。ArduPilot、PX4這些主流飛控都拿它當(dāng)對(duì)外通信的“官方語言”QGroundControl、Mission Planner這些地面站也是靠它和飛機(jī)對(duì)話。我最早接觸MAVLink是在做一個(gè)無人機(jī)數(shù)據(jù)鏈路網(wǎng)關(guān)的時(shí)候那時(shí)候項(xiàng)目技術(shù)棧是Java需要把飛控發(fā)出來的MAVLink報(bào)文接收下來解析出姿態(tài)、GPS、電量這些信息同時(shí)還要能反向把航點(diǎn)任務(wù)或控制指令封包上傳給飛控。一開始我以為這玩意兒有現(xiàn)成的庫直接調(diào)API就行了但實(shí)際一深入才發(fā)現(xiàn)如果不懂幀結(jié)構(gòu)、不懂CRC_EXTRA校驗(yàn)機(jī)制、不懂粘包半包的處理哪怕用了庫也很容易在數(shù)據(jù)交互上翻車。這篇文章就把我對(duì)MAVLink協(xié)議解析與封包的經(jīng)驗(yàn)完整梳理一遍包含協(xié)議核心原理、手寫解析和封包的Java代碼實(shí)現(xiàn)、基于官方庫的工程化用法以及我在實(shí)際調(diào)試中踩過的各種坑。無論你是準(zhǔn)備在Java后端里對(duì)接飛控?cái)?shù)據(jù)還是在做自己的地面站、數(shù)傳中轉(zhuǎn)服務(wù)或者只是對(duì)MAVLink協(xié)議本身感興趣這篇文章應(yīng)該都能幫你少走不少?gòu)澛贰?. Java做MAVLink二次開發(fā)的場(chǎng)景與選型思考1.1 什么場(chǎng)景下會(huì)用到Java對(duì)接MAVLink很多人一聽到MAVLink就想到C或Python畢竟飛控固件和大量開源地面站都是用C寫的Python也靠pymavlink占了腳本自動(dòng)化半壁江山。但在真實(shí)工程里Java接MAVLink的需求其實(shí)一點(diǎn)不少。舉個(gè)例子很多無人機(jī)巡檢、測(cè)繪、安防系統(tǒng)都是前后端分離的Web架構(gòu)后端服務(wù)可能是一個(gè)Spring Boot應(yīng)用負(fù)責(zé)接收飛控回傳的實(shí)時(shí)狀態(tài)寫入時(shí)序數(shù)據(jù)庫再推送給前端Web頁面展示航跡和姿態(tài)。這種情況下鏈路可以是飛控 - 數(shù)傳模塊 - 串口服務(wù)器 - Java后端也可以是飛控 - 機(jī)載計(jì)算機(jī)樹莓派/Jetson上的Java程序 - 云平臺(tái)。還有一類是仿真測(cè)試系統(tǒng)用Java模擬多個(gè)無人機(jī)節(jié)點(diǎn)向地面站或空管系統(tǒng)發(fā)送MAVLink報(bào)文這種場(chǎng)景也需要一套可靠的封包能力。在這些場(chǎng)景里用Java處理MAVLink就不是可選項(xiàng)而是必經(jīng)之路。雖然可以用Java調(diào)用pymavlink的子進(jìn)程繞一圈但性能和部署復(fù)雜度都不理想直接在Java層面實(shí)現(xiàn)協(xié)議解析和封包才是正解。1.2 自己手寫還是用現(xiàn)成庫這是一個(gè)繞不開的選型問題。Java生態(tài)里比較常用的MAVLink庫是來自Dronefleet的mavlink庫以及dronekit-java這類偏應(yīng)用層的SDK。它們能幫你省掉協(xié)議編解碼的臟活但用起來有幾個(gè)限制生成代碼的消息定義通常是固定的如果你用的飛控固件版本較老或較新消息字段可能與庫內(nèi)置的XML定義不一致。自定義MAVLink消息需要自己擴(kuò)展代碼生成器配置成本不低。庫只負(fù)責(zé)編解碼不負(fù)責(zé)傳輸層。你依然要自己處理串口、TCP、UDP的收發(fā)以及半包、粘包、超時(shí)重傳這些實(shí)際問題。所以我個(gè)人的建議是如果你只是想快速跑通一個(gè)Demo直接用現(xiàn)成庫沒問題但如果你想深入理解協(xié)議細(xì)節(jié)或者你的場(chǎng)景有定制消息、私有擴(kuò)展字段、異構(gòu)設(shè)備接入的需求那最好還是把協(xié)議本身吃透甚至自己手寫一套輕量級(jí)的編解碼器。這篇博文的核心部分就圍繞著手寫解析和封包展開同時(shí)也會(huì)給出用官方庫的工程化方案。2. MAVLink幀結(jié)構(gòu)拆解V1/V2差異與CRC_EXTRA設(shè)計(jì)意圖2.1 MAVLink消息幀的基礎(chǔ)布局MAVLink是一種二進(jìn)制協(xié)議不是文本協(xié)議所以解析的時(shí)候沒有換行符、分隔符這種東西可以依賴必須按字節(jié)偏移精確切分。一條完整的MAVLink消息幀大致可以分為三個(gè)區(qū)域幀頭、負(fù)載Payload、幀尾校驗(yàn)。V1版本的幀結(jié)構(gòu)是字節(jié)偏移字段名長(zhǎng)度說明0起始標(biāo)志1固定0xFE1負(fù)載長(zhǎng)度1Payload字節(jié)數(shù)2包序列號(hào)10-255循環(huán)3系統(tǒng)ID1發(fā)送設(shè)備系統(tǒng)編號(hào)4組件ID1發(fā)送設(shè)備組件編號(hào)5消息ID1消息類型6~N負(fù)載數(shù)據(jù)變化具體消息字段N1~N2校驗(yàn)和2CRC16/MCRF4XXV2版本在V1基礎(chǔ)上擴(kuò)展了幀頭起始標(biāo)志變成了0xFD并在消息ID區(qū)域從1字節(jié)擴(kuò)展到了3字節(jié)同時(shí)增加了額外的增量校驗(yàn)字節(jié)Increment/Compatibility flags。V2的幀頭是這樣的字節(jié)偏移字段名長(zhǎng)度說明0起始標(biāo)志1固定0xFD1負(fù)載長(zhǎng)度1Payload字節(jié)數(shù)2包序列號(hào)10-255循環(huán)3系統(tǒng)ID1發(fā)送設(shè)備系統(tǒng)編號(hào)4組件ID1發(fā)送設(shè)備組件編號(hào)5消息ID低字節(jié)1消息ID bit0-76消息ID中間字節(jié)1消息ID bit8-157消息ID高字節(jié)1消息ID bit16-238兼容性標(biāo)志1保留位9兼容性掩碼1保留位10~N9負(fù)載數(shù)據(jù)變化具體消息字段N10~N11校驗(yàn)和2CRC16/MCRF4XX這兩者的核心區(qū)別一個(gè)是消息ID的寬度V2最多支持16777216種消息類型而V1只有256種。另一個(gè)是V2增加了兩個(gè)標(biāo)志字節(jié)方便后續(xù)協(xié)議演進(jìn)。實(shí)際開發(fā)中V1和V2經(jīng)常會(huì)混用所以解析器必須同時(shí)兼容0xFE和0xFD兩種起始標(biāo)志。2.2 系統(tǒng)ID和組件ID到底怎么理解我剛接觸MAVLink時(shí)對(duì)系統(tǒng)ID和組件ID的概念有點(diǎn)模糊后來調(diào)鏈路時(shí)踩了坑才徹底明白。簡(jiǎn)單說系統(tǒng)ID用于區(qū)分同一通信鏈路上的不同飛行器或地面設(shè)備比如你同時(shí)控制3臺(tái)無人機(jī)那3臺(tái)飛機(jī)的系統(tǒng)ID一般會(huì)設(shè)成1、2、3。組件ID則用于區(qū)分同一個(gè)系統(tǒng)內(nèi)部的不同功能模塊比如自動(dòng)飛控組件、GPS模塊、相機(jī)云臺(tái)、地面站軟件它們的組件ID都不同。地面站和飛控通信時(shí)飛控發(fā)出的數(shù)據(jù)里系統(tǒng)ID是飛控自己的編號(hào)組件ID是各個(gè)板載組件的編號(hào)。而地面站要發(fā)送指令給飛控時(shí)發(fā)送方的系統(tǒng)ID必須和飛控配置中認(rèn)定的地面站系統(tǒng)ID一致否則飛控會(huì)直接忽略。這部分看起來不難但做多機(jī)場(chǎng)景時(shí)經(jīng)常有人把系統(tǒng)ID搞混導(dǎo)致指令發(fā)給1號(hào)機(jī)結(jié)果2號(hào)機(jī)沒有反應(yīng)。2.3 CRC_EXTRA很多人忽略卻致命的校驗(yàn)字節(jié)MAVLink的校驗(yàn)和計(jì)算方式跟普通CRC不太一樣它并不是單純對(duì)幀頭加負(fù)載做CRC16。實(shí)際算法是把整條消息幀的數(shù)據(jù)幀頭部分按特定規(guī)則參與不同版本參與的數(shù)據(jù)范圍不同和一個(gè)消息專屬的CRC_EXTRA種子值一起做CRC16/MCRF4XX運(yùn)算。這個(gè)CRC_EXTRA值是從消息定義XML中提取的具體數(shù)值與消息的字段順序、字段類型強(qiáng)相關(guān)。如果發(fā)送端和接收端對(duì)同一消息ID的字段定義不完全一致計(jì)算出的CRC_EXTRA就不一樣接收端理所當(dāng)然會(huì)把這條消息視為非法。以前和第三方設(shè)備做聯(lián)調(diào)時(shí)對(duì)方聲稱支持MAVLink結(jié)果發(fā)過來的消息我們這邊全部校驗(yàn)失敗最后排查發(fā)現(xiàn)對(duì)方的消息定義里把某個(gè)uint32_t字段改成了uint16_t導(dǎo)致CRC_EXTRA種子對(duì)不上。所以在手寫封包和解析時(shí)CRC_EXTRA值必須和你的目標(biāo)飛控固件所用的消息定義嚴(yán)格一致。ArduPilot和PX4的Common.xml定義可以從官方倉庫獲取改動(dòng)任何字段后都要重新計(jì)算CRC_EXTRA。3. 自己寫封包校驗(yàn)計(jì)算、字節(jié)序與代碼實(shí)現(xiàn)3.1 封包前必須先確認(rèn)的兩個(gè)參數(shù)目標(biāo)系統(tǒng)的字節(jié)序與CRC種子動(dòng)手寫封包代碼前先把兩個(gè)參數(shù)確認(rèn)好否則后面調(diào)試會(huì)非常痛苦。第一個(gè)是字節(jié)序。MAVLink協(xié)議規(guī)定所有多字節(jié)數(shù)值類型如uint16_t、uint32_t、float等在負(fù)載區(qū)都使用小端序Little-Endian存儲(chǔ)。這個(gè)規(guī)定幾乎所有飛控實(shí)現(xiàn)都遵循但如果是對(duì)接某些非標(biāo)準(zhǔn)的定制設(shè)備最好先確認(rèn)一下對(duì)方是否符合標(biāo)準(zhǔn)因?yàn)樽止?jié)序錯(cuò)了的話解析出來的數(shù)值會(huì)非常離譜比如高度變成幾百萬米。第二個(gè)是CRC種子。每個(gè)消息ID都對(duì)應(yīng)一個(gè)固定的CRC_EXTRA值這個(gè)值可以通過官方腳本crc_extra.py或在線工具查也可以從MAVLink XML消息定義中提取。我習(xí)慣把用到的消息ID和CRC_EXTRA值維護(hù)在一個(gè)常量類里作為封包和解析共用的對(duì)照表。3.2 手寫封包的核心流程封包的本質(zhì)就是把消息對(duì)象的字段按定義順序?qū)懭胍粋€(gè)字節(jié)緩沖區(qū)計(jì)算CRC再加上幀頭最后輸出完整的字節(jié)數(shù)組。以發(fā)送V1版本HEARTBEAT消息為例我的實(shí)現(xiàn)思路大致如下創(chuàng)建一個(gè)ByteBuffer設(shè)置為小端序如果字節(jié)序不匹配這里就要先調(diào)整否則后面所有數(shù)值字段都會(huì)出問題。預(yù)留幀頭部分的空間或者先把幀頭字段逐個(gè)寫入。由于負(fù)載長(zhǎng)度是先知的所以可以先把負(fù)載長(zhǎng)度、包序列號(hào)、系統(tǒng)ID、組件ID、消息ID依次寫入緩沖區(qū)。接下來寫入負(fù)載數(shù)據(jù)也就是消息具體字段。幀頭加負(fù)載全部寫入后用這份完整數(shù)據(jù)計(jì)算CRC16校驗(yàn)和再把2字節(jié)校驗(yàn)和追加到緩沖區(qū)末尾。最終輸出byte[]就是一條可發(fā)送的MAVLink消息幀。下面是一個(gè)偏基礎(chǔ)的實(shí)現(xiàn)示例目標(biāo)是可讀性優(yōu)先方便大家理解協(xié)議流程實(shí)際工程中可以根據(jù)需要再優(yōu)化。import java.nio.ByteBuffer; import java.nio.ByteOrder; public class MavlinkV1Frame { // 每個(gè)消息ID對(duì)應(yīng)的CRC_EXTRA值這里用HEARTBEAT舉例 private static final int CRC_EXTRA_HEARTBEAT 50; public static byte[] buildHeartbeatV1(int sysId, int compId, int seq) { // HEARTBEAT V1 負(fù)載長(zhǎng)度9 字節(jié) int payloadLength 9; ByteBuffer buf ByteBuffer.allocate(payloadLength 8); buf.order(ByteOrder.LITTLE_ENDIAN); buf.put((byte) 0xFE); // 起始標(biāo)志 buf.put((byte) payloadLength); // 負(fù)載長(zhǎng)度 buf.put((byte) seq); // 包序列號(hào) buf.put((byte) sysId); // 系統(tǒng)ID buf.put((byte) compId); // 組件ID buf.put((byte) 0); // 消息ID: HEARTBEAT // payload: MAV_TYPE_FIXED_WING1, MAV_AUTOPILOT_ARDUPILOTMEGA3 buf.put((byte) 1); // type buf.put((byte) 3); // autopilot buf.put((byte) 0); // base_mode buf.put((byte) 0); // custom_mode 低字節(jié) buf.put((byte) 0); // custom_mode 高字節(jié) buf.put((byte) 0); // system_status buf.put((byte) 0); // mavlink_version byte[] frame buf.array(); // 計(jì)算CRC16幀頭 負(fù)載并追加CRC_EXTRA種子參與運(yùn)算 int crc Crc16Util.crc16(frame, frame.length - 2); crc Crc16Util.accumulateByte(crc, CRC_EXTRA_HEARTBEAT); ByteBuffer result ByteBuffer.allocate(frame.length 2); result.order(ByteOrder.LITTLE_ENDIAN); result.put(frame); result.putShort((short) (crc 0xFFFF)); // 校驗(yàn)和低字節(jié)在前 return result.array(); } }這里的Crc16Util就是CRC16/MCRF4XX算法的具體實(shí)現(xiàn)。關(guān)于算法本身下文會(huì)詳細(xì)展開這里先留一個(gè)引用。3.3 CRC16/MCRF4XX怎么用Java實(shí)現(xiàn)MAVLink用的CRC16是CRC16/MCRF4XX它的多項(xiàng)式是0x1021初始值是0xFFFF。算法對(duì)輸入數(shù)據(jù)的每一位進(jìn)行處理和普通CRC16區(qū)別不大關(guān)鍵是MAVLink協(xié)議規(guī)定CRC結(jié)束后還需要把CRC_EXTRA種子當(dāng)作一個(gè)獨(dú)立字節(jié)繼續(xù)參與一次或多次累加運(yùn)算。以下是Java實(shí)現(xiàn)可以直接復(fù)用public class Crc16Util { public static int crc16(byte[] data) { int crc 0xFFFF; for (byte b : data) { crc ^ (b 0xFF) 8; for (int i 0; i 8; i) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } } return crc; } public static int crc16(byte[] data, int length) { int crc 0xFFFF; for (int i 0; i length; i) { crc ^ (data[i] 0xFF) 8; for (int j 0; j 8; j) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } } return crc; } public static int accumulateByte(int crc, int byteValue) { crc ^ (byteValue 0xFF) 8; for (int j 0; j 8; j) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } return crc; } }我看到有些網(wǎng)上流傳的代碼在累加CRC_EXTRA時(shí)會(huì)忽略字節(jié)的高位其實(shí)那是不嚴(yán)格的。accumulateByte方法里必須把傳入值按0xFF取低位再放到高8位參與運(yùn)算這樣才能和官方實(shí)現(xiàn)保持一致。3.4 字節(jié)填充順序與ByteBuffer的選擇寫封包代碼時(shí)我最常犯的錯(cuò)誤是字段順序和官方消息定義不一致。MAVLink消息的負(fù)載字段順序不是隨意排的而是XML定義里從上到下的順序。比如GLOBAL_POSITION_INT的定義順序是time_boot_ms、lat、lon、alt、relative_alt、vx、vy、vz、hdg如果你把lat和lon寫反了經(jīng)緯度就會(huì)在圖上走出奇怪的軌跡。Java里處理二進(jìn)制流我推薦用ByteBuffer而不是手工拼接byte數(shù)組。原因有三點(diǎn)一是ByteBuffer原生支持小端序轉(zhuǎn)換只要order(ByteOrder.LITTLE_ENDIAN)設(shè)好putInt、putFloat這些方法會(huì)自動(dòng)處理字節(jié)順序二是它支持批量操作可以避免大量數(shù)組復(fù)制三是調(diào)試時(shí)可以直接看position和remaining方便定位寫入位置。不過要注意ByteBuffer的get/put操作會(huì)把內(nèi)部position向前移動(dòng)讀取時(shí)需要有意識(shí)的flip或使用絕對(duì)位置訪問。3.5 V2封包的差異點(diǎn)如果你要跟PX4或較新的ArduPilot通信V2封包更常見。V2和V1的封包流程差別不大但有幾個(gè)點(diǎn)要特別注意起始標(biāo)志是0xFD接收方如果只認(rèn)0xFE需要兩個(gè)分支同時(shí)兼容。消息ID是3字節(jié)寫入時(shí)按低、中、高的順序依次put。在幀頭多出的兩個(gè)標(biāo)志字節(jié)通常填0表示沒有開啟額外的簽名或兼容性擴(kuò)展。如果開啟了消息簽名功能幀尾除了2字節(jié)CRC還會(huì)追加13字節(jié)的簽名數(shù)據(jù)簽名機(jī)制比較復(fù)雜一般用不到但對(duì)接某些安全要求較高的系統(tǒng)時(shí)可能要處理。在實(shí)際工程里我建議優(yōu)先支持V2發(fā)送同時(shí)兼容V1解析因?yàn)榇蠖鄶?shù)現(xiàn)代飛控都兼容V2接收而很多舊設(shè)備還在發(fā)V1。4. 自己寫解析流式數(shù)據(jù)的狀態(tài)機(jī)處理4.1 粘包、半包與起始標(biāo)志誤判寫解析器時(shí)第一個(gè)要面對(duì)的問題就是MAVLink是流式協(xié)議底層可能是串口、TCP或UDP數(shù)據(jù)到達(dá)時(shí)不會(huì)剛好是一條完整的消息邊界。你可能會(huì)一次性收到好幾條消息粘在一起也可能收到一條消息的前半截過一會(huì)才收到后半截。最簡(jiǎn)單的處理思路就是一個(gè)字節(jié)一個(gè)字節(jié)地喂進(jìn)狀態(tài)機(jī)每完整收齊一幀就丟出一幀。這種方式雖然看起來笨拙但勝在穩(wěn)定不容易丟包而且實(shí)現(xiàn)邏輯直觀。效率上對(duì)于無人機(jī)數(shù)據(jù)鏈路這種吞吐量完全夠用。4.2 狀態(tài)機(jī)設(shè)計(jì)我把解析狀態(tài)機(jī)劃分成以下狀態(tài)WAITING_FOR_START等待起始標(biāo)志0xFE或0xFD。READING_HEADER正在讀取幀頭剩余字段根據(jù)起始標(biāo)志區(qū)分V1和V2的幀頭長(zhǎng)度。READING_PAYLOAD按幀頭聲明的負(fù)載長(zhǎng)度讀取負(fù)載數(shù)據(jù)。CHECKING_CRC讀取2字節(jié)校驗(yàn)和并計(jì)算比對(duì)?；具壿嬍莗ublic class MavlinkParser { private static final byte START_V1 (byte) 0xFE; private static final byte START_V2 (byte) 0xFD; private int state 0; // 0 等起始, 1 讀幀頭, 2 讀負(fù)載, 3 讀CRC private boolean isV2 false; private int headerLen 0; private int payloadLen 0; private int crcExtra 0; private byte[] header new byte[12]; private int headerPos 0; private byte[] payload new byte[255]; private int payloadPos 0; private int seq 0; private int sysId 0; private int compId 0; private int msgId 0; public void feed(byte b, ListMavlinkMessage out) { switch (state) { case 0: if (b START_V1) { isV2 false; headerLen 6; headerPos 0; state 1; } else if (b START_V2) { isV2 true; headerLen 10; headerPos 0; state 1; } break; case 1: header[headerPos] b; if (headerPos 1) { payloadLen b 0xFF; } if (headerPos headerLen) { // 解析幀頭字段 if (isV2) { seq header[2] 0xFF; sysId header[3] 0xFF; compId header[4] 0xFF; msgId (header[5] 0xFF) | ((header[6] 0xFF) 8) | ((header[7] 0xFF) 16); } else { seq header[2] 0xFF; sysId header[3] 0xFF; compId header[4] 0xFF; msgId header[5] 0xFF; } payloadPos 0; state 2; } break; case 2: if (payloadPos payloadLen) { payload[payloadPos] b; } if (payloadPos payloadLen) { state 3; } break; case 3: // 先讀到的低字節(jié)后讀到的高字節(jié) int crcLow b 0xFF; // 這里簡(jiǎn)化處理實(shí)際還需要第二個(gè)字節(jié)才能算完 // 建議把CRC作為一個(gè)兩字節(jié)小狀態(tài)再細(xì)分 break; } } }上面的代碼只是一個(gè)骨架實(shí)際用的時(shí)候需要把CRC校驗(yàn)部分拆成兩步先收crcLow再收crcHigh收齊后計(jì)算整幀數(shù)據(jù)的CRC然后把crcLow和crcHigh拼成int和計(jì)算值比對(duì)。為了方便閱讀我沒有把CRC子狀態(tài)寫進(jìn)去但思路是一致的。4.3 校驗(yàn)失敗怎么辦CRC校驗(yàn)失敗時(shí)最簡(jiǎn)單粗暴的做法就是把當(dāng)前這一幀丟棄然后回到WAITING_FOR_START狀態(tài)重新開始。這個(gè)策略在大多數(shù)字鏈路場(chǎng)景下是合適的因?yàn)镸AVLink本身沒有幀級(jí)重傳機(jī)制漏掉一幀姿態(tài)數(shù)據(jù)下一幀馬上就會(huì)補(bǔ)上實(shí)時(shí)性數(shù)據(jù)對(duì)單幀丟失并不敏感。但有一種情況要特別注意如果在等待CRC的連續(xù)兩個(gè)字節(jié)期間因?yàn)榇诓ㄌ芈逝渲貌粚?duì)或者鏈路干擾造成字節(jié)錯(cuò)位那么狀態(tài)機(jī)可能會(huì)進(jìn)入假同步。比如數(shù)據(jù)里恰好出現(xiàn)0xFE被你當(dāng)成起始標(biāo)志隨后幾字節(jié)又恰好滿足幀頭結(jié)構(gòu)結(jié)果組裝出一條校驗(yàn)失敗的假消息。處理辦法是CRC校驗(yàn)失敗后不要簡(jiǎn)單地把丟棄點(diǎn)之后的第一個(gè)0xFE當(dāng)作新起始而是應(yīng)該退回到起始標(biāo)志狀態(tài)同時(shí)保留當(dāng)前緩沖區(qū)里已經(jīng)讀過的數(shù)據(jù)從下一位繼續(xù)掃描。否則某些干擾環(huán)境下會(huì)出現(xiàn)頻繁丟真幀的現(xiàn)象。4.4 如何從原始字節(jié)流切出完整幀半包場(chǎng)景實(shí)測(cè)假設(shè)你從串口讀到了這樣一個(gè)字節(jié)流FE 09 01 01 01 00 ...后面還缺了20個(gè)字節(jié)才夠完整。這時(shí)狀態(tài)機(jī)會(huì)停在READING_PAYLOAD或CHECKING_CRC狀態(tài)。等下一批數(shù)據(jù)到達(dá)你要把新數(shù)據(jù)繼續(xù)喂給狀態(tài)機(jī)而不是重新開一幀。這就是狀態(tài)機(jī)的好處幀狀態(tài)跨批次保持天然處理了半包。反之如果一次讀到了三幀粘在一起狀態(tài)機(jī)也會(huì)在解析完第一幀后立刻把后續(xù)的0xFE識(shí)別為新幀的起始繼續(xù)解析第二幀、第三幀。整個(gè)過程不需要任何復(fù)雜緩沖區(qū)分割一個(gè)狀態(tài)機(jī)加一個(gè)回調(diào)接口就夠。在實(shí)際項(xiàng)目中我通常會(huì)在狀態(tài)機(jī)外面包一個(gè)parse(byte[] data)方法循環(huán)調(diào)用feed并把解析出來的完整消息收集到List返回。這樣一個(gè)方法就能統(tǒng)一處理單包多包、半包全包的全部場(chǎng)景。5. 工程化接入使用官方Java庫構(gòu)建數(shù)據(jù)交互鏈路5.1 用庫不等于不學(xué)協(xié)議自己手寫解析器非常適合學(xué)習(xí)協(xié)議但工程效率方面如果目標(biāo)只是快速跑通業(yè)務(wù)我還是推薦用官方生態(tài)的Java庫。dronefleet的mavlink庫是業(yè)界相對(duì)活躍的Java MAVLink庫它通過代碼生成器從XML消息定義生成Java類消息類型全CRC_EXTRA也提前算好了省去自己維護(hù)一大堆常量。不過注意用庫之前最好還是把我前面講的那套幀結(jié)構(gòu)、校驗(yàn)機(jī)制理解清楚因?yàn)閹熘荒軒湍憬鉀Q編解碼問題它不負(fù)責(zé)處理串口或網(wǎng)絡(luò)傳輸。數(shù)據(jù)從串口讀出來后該面對(duì)的半包、粘包問題依然要自己面對(duì)。5.2 引入依賴并發(fā)送HEARTBEAT消息以Maven項(xiàng)目為例引入dronefleet mavlink庫dependency groupIdio.dronefleet.mavlink/groupId artifactIdmavlink-core/artifactId version1.1.0/version /dependency如果是ArduPilot再加一個(gè)dependency groupIdio.dronefleet.mavlink/groupId artifactIdmavlink-ardupilotmega/artifactId version1.1.0/version /dependency有了依賴后發(fā)送HEARTBEAT消息的代碼大致如下import io.dronefleet.mavlink.MavlinkConnection; import io.dronefleet.mavlink.MavlinkMessage; import io.dronefleet.mavlink.common.Heartbeat; import io.dronefleet.mavlink.common.MavAutopilot; import io.dronefleet.mavlink.common.MavState; import io.dronefleet.mavlink.common.MavType; import java.io.ByteArrayInputStream; import java.io.ByteArrayOutputStream; public class MavlinkSender { private MavlinkConnection connection; public MavlinkSender() { ByteArrayOutputStream out new ByteArrayOutputStream(); ByteArrayInputStream in new ByteArrayInputStream(new byte[0]); connection MavlinkConnection.create(in, out); } public byte[] buildHeartbeat() { Heartbeat heartbeat Heartbeat.builder() .type(MavType.MAV_TYPE_FIXED_WING) .autopilot(MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA) .baseMode(0) .customMode(0) .systemStatus(MavState.MAV_STATE_STANDBY) .mavlinkVersion(3) .build(); MavlinkMessage? msg MavlinkMessage.create(1, 1, heartbeat); connection.send(msg, msg.getOriginSystemId(), msg.getOriginComponentId()); try { return out.toByteArray(); } finally { out.reset(); } } }這段代碼的關(guān)鍵邏輯在MavlinkConnection.send方法內(nèi)部它幫你處理了消息幀的組裝、CRC_EXTRA的寫入、序列號(hào)的維護(hù)。getOriginSystemId和getOriginComponentId分別是消息對(duì)象攜帶的系統(tǒng)ID和組件ID需要根據(jù)你的設(shè)備編號(hào)設(shè)置。5.3 解析庫的常見處理方式用庫解析消息時(shí)流程一般是從串口或網(wǎng)絡(luò)讀取原始字節(jié)。調(diào)用MavlinkConnection的next()方法傳入原始字節(jié)流返回解析出的MavlinkMessage對(duì)象。對(duì)返回的消息對(duì)象做類型匹配判斷是HEARTBEAT還是GLOBAL_POSITION_INT再?gòu)?qiáng)轉(zhuǎn)成具體類型取字段。一個(gè)簡(jiǎn)單的解析循環(huán)如下import io.dronefleet.mavlink.MavlinkConnection; import io.dronefleet.mavlink.MavlinkMessage; import io.dronefleet.mavlink.common.GlobalPositionInt; import io.dronefleet.mavlink.common.Heartbeat; import java.io.ByteArrayInputStream; public class MavlinkReceiver { public void handle(byte[] data) { ByteArrayInputStream in new ByteArrayInputStream(data); MavlinkConnection connection MavlinkConnection.create(in, new ByteArrayOutputStream()); // 注意這里只處理單幀實(shí)際場(chǎng)景需要循環(huán)調(diào)用直到數(shù)據(jù)耗盡 MavlinkMessage? msg connection.next(); if (msg.getMessage() instanceof Heartbeat) { Heartbeat hb (Heartbeat) msg.getMessage(); System.out.println(收到心跳類型: hb.type()); } else if (msg.getMessage() instanceof GlobalPositionInt) { GlobalPositionInt gpi (GlobalPositionInt) msg.getMessage(); System.out.println(緯度: gpi.lat() / 1.0E7); System.out.println(經(jīng)度: gpi.lon() / 1.0E7); System.out.println(高度: gpi.alt() / 1000.0); } } }這里的lat和lon是int類型單位是1e7度alt單位是毫米換算成常用單位時(shí)記得除以對(duì)應(yīng)系數(shù)。用庫雖然方便但協(xié)議層面的單位換算還是得自己心里有數(shù)。5.4 串口、TCP、UDP傳輸層怎么選MAVLink本身是獨(dú)立的協(xié)議層底層傳輸可以用UART串口、TCP或UDP。串口場(chǎng)景飛控通過數(shù)傳模塊連到地面站或工控機(jī)數(shù)據(jù)以字節(jié)流方式到達(dá)通常用jserialcomm或串口框架讀取。串口沒有消息邊界必須靠狀態(tài)機(jī)處理。TCP場(chǎng)景常見于數(shù)傳模塊通過TCP轉(zhuǎn)發(fā)數(shù)據(jù)或者機(jī)載計(jì)算機(jī)和地面站之間走TCP長(zhǎng)連接。TCP自帶流控和重傳但依然存在粘包問題處理方式和串口一樣。UDP場(chǎng)景常用于局域網(wǎng)內(nèi)的數(shù)據(jù)分發(fā)比如QGroundControl和多個(gè)地面終端之間的組播轉(zhuǎn)發(fā)。UDP雖然是數(shù)據(jù)報(bào)協(xié)議但MAVLink經(jīng)常把多條消息塞進(jìn)同一個(gè)UDP包所以依然需要按幀切分。無論底層的Transport是什么核心編解碼邏輯都可以復(fù)用同一套狀態(tài)機(jī)或同一套MavlinkConnection。我在工程里一般把傳輸層抽象成一個(gè)Transport接口只提供read和write兩個(gè)方法這樣切換串口和UDP就非常輕量。6. 實(shí)際調(diào)試中踩過的坑與定位方法6.1 坑一CRC_EXTRA值對(duì)不上消息被靜默丟棄有一次聯(lián)調(diào)對(duì)方用自研設(shè)備發(fā)HEARTBEAT我們用解析器接收解析器始終打印“CRC校驗(yàn)失敗”。查看抓包數(shù)據(jù)幀頭、負(fù)載、長(zhǎng)度都完全正確但CRC就是不對(duì)。反復(fù)核對(duì)后才發(fā)現(xiàn)對(duì)方在生成消息定義時(shí)把system_status字段從uint8_t改成了uint16_t。字段類型變化導(dǎo)致負(fù)載長(zhǎng)度和CRC_EXTRA都變了幀頭里寫的負(fù)載長(zhǎng)度是新長(zhǎng)度但CRC_EXTRA用的是舊值整個(gè)校驗(yàn)自然失敗。這個(gè)坑教會(huì)我一件事CRC_EXTRA不只是一個(gè)消息ID對(duì)應(yīng)的常量它本身就是消息定義的指紋。任何字段順序、類型、長(zhǎng)度的變化都會(huì)改變它。所以聯(lián)調(diào)時(shí)如果CRC瘋狂失敗先不要懷疑算法實(shí)現(xiàn)先核對(duì)雙方的消息定義XML是否完全一致。這比一行行看代碼高效得多。6.2 坑二串口讀數(shù)時(shí)buffer不夠?qū)е聨財(cái)嘤么谧x數(shù)時(shí)如果讀取緩沖區(qū)設(shè)置得比較小可能出現(xiàn)一次讀到的數(shù)據(jù)只是一幀消息的一半另一半還留在串口緩沖區(qū)里。如果讀取代碼寫得比較簡(jiǎn)單比如讀一次就嘗試解析那就會(huì)出現(xiàn)大量半包導(dǎo)致的解析失敗。我的處理方案是讀到的每一批數(shù)據(jù)都先追加到一個(gè)公共的緩沖隊(duì)列然后讓狀態(tài)機(jī)持續(xù)消費(fèi)緩沖隊(duì)列里的數(shù)據(jù)直到隊(duì)列為空。不要每讀到一批數(shù)據(jù)就做一次“起點(diǎn)尋找完整校驗(yàn)”那是錯(cuò)誤姿勢(shì)。6.3 坑三非小端序造成的臟數(shù)據(jù)Java在x86平臺(tái)上默認(rèn)是大端序執(zhí)行ByteBuffer.wrap(bytes).getInt()讀出的數(shù)據(jù)在MAVLink場(chǎng)景里往往會(huì)得到一個(gè)很大的數(shù)。很多新手在里踩了坑之后以為是自己解析錯(cuò)位了其實(shí)只是忘了設(shè)置ByteOrder.LITTLE_ENDIAN。在涉及float字段時(shí)這個(gè)坑更隱蔽。比如高度、速度、姿態(tài)角這些字段很多是float類型如果字節(jié)序不對(duì)解析出來的數(shù)值看起來不是邏輯錯(cuò)誤而是一些非常奇怪的小數(shù)。遇到這種數(shù)值不要懷疑人生先檢查ByteOrder。6.4 坑四消息序列號(hào)與丟包統(tǒng)計(jì)MAVLink幀頭里的包序列號(hào)seq可以用來統(tǒng)計(jì)鏈路丟包率。我在做鏈路質(zhì)量檢測(cè)時(shí)會(huì)記錄每個(gè)系統(tǒng)ID組件ID組合的最近一次序列號(hào)如果下一次收到的序列號(hào)與期望值不連續(xù)就說明中間有丟幀。這個(gè)功能看著簡(jiǎn)單但沒有序列號(hào)意識(shí)的人往往忽略它而它在實(shí)飛調(diào)試中非常有用能快速判斷是數(shù)傳鏈路問題還是解析程序問題。6.5 坑五坐標(biāo)系與單位換算MAVLink里的經(jīng)緯度使用的是整數(shù)類型單位是1e7度也就是說lat373774819代表緯度37.3774819度。高度字段可能是毫米空速可能是厘米每秒地速可能是厘米每秒角速度可能是厘度每秒。不同消息的字段單位差異巨大尤其是從飛控切換到PX4再到ArduPilot時(shí)某些擴(kuò)展消息的單位還會(huì)變化。我在做數(shù)據(jù)展示時(shí)會(huì)把單位換算集中到一個(gè)轉(zhuǎn)換層不散落到各個(gè)業(yè)務(wù)方法里這樣即使后續(xù)協(xié)議單位發(fā)生變化只需要改轉(zhuǎn)換層一處。7. 用仿真環(huán)境驗(yàn)證整條鏈路7.1 搭建一個(gè)本地MAVLink驗(yàn)證環(huán)境代碼寫完了怎么驗(yàn)證封包和解析是否正確我的習(xí)慣是先用MAVProxy或者QGroundControl連接一個(gè)SITL仿真飛控這樣不炸機(jī)也能把整個(gè)鏈路跑通。SITL是ArduPilot的軟件在環(huán)仿真可以在PC上虛擬出一臺(tái)完整飛控。啟動(dòng)后SITL會(huì)在本地UDP端口14550上輸出MAVLinkQGroundControl默認(rèn)也能連這個(gè)端口。如果你只想驗(yàn)證自己寫的Java解析器可以讓Java程序監(jiān)聽UDP 14550端口把收到的UDP數(shù)據(jù)包交給狀態(tài)機(jī)解析打印出HEARTBEAT和GLOBAL_POSITION_INT信息。如果程序能持續(xù)打印出正常的經(jīng)緯度和高度說明解析鏈路是通的。7.2 發(fā)送航點(diǎn)消息給仿真飛控的驗(yàn)證步驟如果你想驗(yàn)證封包能力比如發(fā)送MissionItem消息給飛控也可以直接用SITL來驗(yàn)證。流程是啟動(dòng)SITL仿真。用你的Java程序向SITL的UDP端口發(fā)送MISSION_COUNT、MISSION_ITEM最后發(fā)送MISSION_START等消息。在QGroundControl地面站上觀察航點(diǎn)是否被接收并可以執(zhí)行。要注意的是ArduPilot對(duì)航點(diǎn)消息有一套握手流程不是直接發(fā)一個(gè)MISSION_ITEM就能完成的。你需要按順序處理MISSION_REQUEST_LIST等往返消息否則飛控不會(huì)接受航點(diǎn)。很多人第一次做航點(diǎn)上傳時(shí)在這里卡了很久建議直接看官方MAVLink的Mission Protocol狀態(tài)機(jī)圖搞清楚每個(gè)狀態(tài)該發(fā)什么消息。7.3 自定義消息擴(kuò)展的思路如果你有特殊的數(shù)據(jù)需要走M(jìn)AVLink比如機(jī)載AI識(shí)別結(jié)果、私有傳感器數(shù)據(jù)正規(guī)做法是自定義MAVLink消息擴(kuò)展。思路是在common.xml或ardupilotmega.xml中新增消息定義分配一個(gè)未使用的消息ID。運(yùn)行代碼生成器重新生成Java類。用新的Java類編解碼自定義消息。整個(gè)過程不算簡(jiǎn)單但可行性很高。ArduPilot固件側(cè)也可以注冊(cè)對(duì)應(yīng)的自定義消息處理邏輯。如果你不想改飛控固件還可以使用MAVLink的MAV_CMD、NAMED_VALUE_FLOAT、DEBUG_VECT等通用消息來傳輸自定義數(shù)據(jù)這些消息在常見飛控固件里都有支持不需要改動(dòng)固件。8. 調(diào)試工具與效率提升建議8.1 抓包工具比打印日志好用得多解析MAVLink時(shí)遇到詭異問題我強(qiáng)烈建議先用抓包工具看原始二進(jìn)制而不是在代碼里瘋狂打印日志。可以在電腦上用Wireshark抓UDP或串口數(shù)據(jù)包再用4243端口或串口監(jiān)控工具把二進(jìn)制流導(dǎo)出成hex文件。hex文件一看起始標(biāo)志、幀長(zhǎng)度、消息ID是否連續(xù)、CRC是否規(guī)律性失敗基本一目了然。Java程序里也可以加一個(gè)可開關(guān)的hex dump工具類在高頻調(diào)試階段打開把每個(gè)UDP包或串口讀取到的數(shù)據(jù)以hex格式輸出。聯(lián)調(diào)時(shí)讓對(duì)方也打開同樣的hex dump兩邊一對(duì)比是發(fā)送端的問題還是接收端的問題立刻清楚。8.2 消息日志要帶序列號(hào)和時(shí)間戳日志不能只打“收到HEARTBEAT”這種信息最好把幀頭的seq、系統(tǒng)ID、組件ID、CRC校驗(yàn)結(jié)果一起打出來。seq和時(shí)間戳能幫你定位丟幀規(guī)律系統(tǒng)ID和組件ID能幫你區(qū)分多設(shè)備混連時(shí)的消息來源。這些信息在后續(xù)排查多機(jī)干擾、鏈路偶發(fā)斷連時(shí)都是關(guān)鍵線索。8.3 建立消息ID對(duì)照表如果你接觸的MAVLink消息比較多建議在項(xiàng)目里維護(hù)一個(gè)消息ID和消息名稱的對(duì)照表這樣抓包看到msgId33就能立刻反應(yīng)過來是GLOBAL_POSITION_INT而不是每次都要臨時(shí)查XML定義。這個(gè)表也可以用于日志過濾比如只查看姿態(tài)消息或只查看心跳消息。9. 從單機(jī)到多機(jī)數(shù)據(jù)交互設(shè)計(jì)的一些體會(huì)實(shí)際做多機(jī)場(chǎng)景時(shí)MAVLink的編解碼本身不是最復(fù)雜的部分復(fù)雜的是如何從流式數(shù)據(jù)中區(qū)分不同飛行器的消息。前面說過每架飛機(jī)的系統(tǒng)ID不同所以在解析出消息后第一件事就是根據(jù)源系統(tǒng)ID做路由。我的做法是維護(hù)一個(gè)MapInteger, DeviceSessionkey是系統(tǒng)IDvalue是當(dāng)前在線設(shè)備的狀態(tài)快照。每次解析出一條消息就更新對(duì)應(yīng)設(shè)備的狀態(tài)快照同時(shí)記錄最后活躍時(shí)間。這樣一個(gè)極簡(jiǎn)的設(shè)備管理服務(wù)就能支撐上百臺(tái)無人機(jī)的狀態(tài)監(jiān)控。心跳超時(shí)檢測(cè)也建議做一下。MAVLink的HEARTBEAT默認(rèn)1Hz發(fā)送如果連續(xù)3到5秒沒有收到某個(gè)系統(tǒng)ID的心跳基本可以判定該設(shè)備鏈路中斷。我在服務(wù)里用定時(shí)任務(wù)掃描設(shè)備會(huì)話的最后活躍時(shí)間超過閾值就標(biāo)記離線并推送告警。另外多機(jī)同時(shí)操作時(shí)指令消息必須攜帶正確的目標(biāo)系統(tǒng)ID和目標(biāo)組件ID。MAVLink的大多數(shù)指令消息都有target_system和target_component字段如果填錯(cuò)飛控可能會(huì)忽略指令。真實(shí)項(xiàng)目中因?yàn)閠arget_system填錯(cuò)導(dǎo)致指令發(fā)給另一臺(tái)飛機(jī)的教訓(xùn)太多了所以發(fā)送前一定要確認(rèn)目標(biāo)編號(hào)。從協(xié)議解析到封包發(fā)送從單機(jī)調(diào)試到多機(jī)管理MAVLink這套體系本身并不復(fù)雜但它藏了很多細(xì)節(jié)任何一個(gè)環(huán)節(jié)出問題整條數(shù)據(jù)鏈路都會(huì)變得不可用。希望我上面這些代碼和踩坑經(jīng)驗(yàn)?zāi)軒湍闵僮咭恍澛?。如果你正在做相關(guān)的Java項(xiàng)目遇到具體的協(xié)議細(xì)節(jié)問題也歡迎交流。 Java實(shí)現(xiàn)MAVLink協(xié)議解析與封包：幀結(jié)構(gòu)、CRC校驗(yàn)與工程實(shí)踐
搞過無人機(jī)地面站、數(shù)傳鏈路或者飛控二次開發(fā)的朋友對(duì)MAVLink這個(gè)協(xié)議應(yīng)該都不陌生。ArduPilot、PX4這些主流飛控都拿它當(dāng)對(duì)外通信的“官方語言”QGroundControl、Mission Planner這些地面站也是靠它和飛機(jī)對(duì)話。我最早接觸MAVLink是在做一個(gè)無人機(jī)數(shù)據(jù)鏈路網(wǎng)關(guān)的時(shí)候那時(shí)候項(xiàng)目技術(shù)棧是Java需要把飛控發(fā)出來的MAVLink報(bào)文接收下來解析出姿態(tài)、GPS、電量這些信息同時(shí)還要能反向把航點(diǎn)任務(wù)或控制指令封包上傳給飛控。一開始我以為這玩意兒有現(xiàn)成的庫直接調(diào)API就行了但實(shí)際一深入才發(fā)現(xiàn)如果不懂幀結(jié)構(gòu)、不懂CRC_EXTRA校驗(yàn)機(jī)制、不懂粘包半包的處理哪怕用了庫也很容易在數(shù)據(jù)交互上翻車。這篇文章就把我對(duì)MAVLink協(xié)議解析與封包的經(jīng)驗(yàn)完整梳理一遍包含協(xié)議核心原理、手寫解析和封包的Java代碼實(shí)現(xiàn)、基于官方庫的工程化用法以及我在實(shí)際調(diào)試中踩過的各種坑。無論你是準(zhǔn)備在Java后端里對(duì)接飛控?cái)?shù)據(jù)還是在做自己的地面站、數(shù)傳中轉(zhuǎn)服務(wù)或者只是對(duì)MAVLink協(xié)議本身感興趣這篇文章應(yīng)該都能幫你少走不少?gòu)澛贰?. Java做MAVLink二次開發(fā)的場(chǎng)景與選型思考1.1 什么場(chǎng)景下會(huì)用到Java對(duì)接MAVLink很多人一聽到MAVLink就想到C或Python畢竟飛控固件和大量開源地面站都是用C寫的Python也靠pymavlink占了腳本自動(dòng)化半壁江山。但在真實(shí)工程里Java接MAVLink的需求其實(shí)一點(diǎn)不少。舉個(gè)例子很多無人機(jī)巡檢、測(cè)繪、安防系統(tǒng)都是前后端分離的Web架構(gòu)后端服務(wù)可能是一個(gè)Spring Boot應(yīng)用負(fù)責(zé)接收飛控回傳的實(shí)時(shí)狀態(tài)寫入時(shí)序數(shù)據(jù)庫再推送給前端Web頁面展示航跡和姿態(tài)。這種情況下鏈路可以是飛控 - 數(shù)傳模塊 - 串口服務(wù)器 - Java后端也可以是飛控 - 機(jī)載計(jì)算機(jī)樹莓派/Jetson上的Java程序 - 云平臺(tái)。還有一類是仿真測(cè)試系統(tǒng)用Java模擬多個(gè)無人機(jī)節(jié)點(diǎn)向地面站或空管系統(tǒng)發(fā)送MAVLink報(bào)文這種場(chǎng)景也需要一套可靠的封包能力。在這些場(chǎng)景里用Java處理MAVLink就不是可選項(xiàng)而是必經(jīng)之路。雖然可以用Java調(diào)用pymavlink的子進(jìn)程繞一圈但性能和部署復(fù)雜度都不理想直接在Java層面實(shí)現(xiàn)協(xié)議解析和封包才是正解。1.2 自己手寫還是用現(xiàn)成庫這是一個(gè)繞不開的選型問題。Java生態(tài)里比較常用的MAVLink庫是來自Dronefleet的mavlink庫以及dronekit-java這類偏應(yīng)用層的SDK。它們能幫你省掉協(xié)議編解碼的臟活但用起來有幾個(gè)限制生成代碼的消息定義通常是固定的如果你用的飛控固件版本較老或較新消息字段可能與庫內(nèi)置的XML定義不一致。自定義MAVLink消息需要自己擴(kuò)展代碼生成器配置成本不低。庫只負(fù)責(zé)編解碼不負(fù)責(zé)傳輸層。你依然要自己處理串口、TCP、UDP的收發(fā)以及半包、粘包、超時(shí)重傳這些實(shí)際問題。所以我個(gè)人的建議是如果你只是想快速跑通一個(gè)Demo直接用現(xiàn)成庫沒問題但如果你想深入理解協(xié)議細(xì)節(jié)或者你的場(chǎng)景有定制消息、私有擴(kuò)展字段、異構(gòu)設(shè)備接入的需求那最好還是把協(xié)議本身吃透甚至自己手寫一套輕量級(jí)的編解碼器。這篇博文的核心部分就圍繞著手寫解析和封包展開同時(shí)也會(huì)給出用官方庫的工程化方案。2. MAVLink幀結(jié)構(gòu)拆解V1/V2差異與CRC_EXTRA設(shè)計(jì)意圖2.1 MAVLink消息幀的基礎(chǔ)布局MAVLink是一種二進(jìn)制協(xié)議不是文本協(xié)議所以解析的時(shí)候沒有換行符、分隔符這種東西可以依賴必須按字節(jié)偏移精確切分。一條完整的MAVLink消息幀大致可以分為三個(gè)區(qū)域幀頭、負(fù)載Payload、幀尾校驗(yàn)。V1版本的幀結(jié)構(gòu)是字節(jié)偏移字段名長(zhǎng)度說明0起始標(biāo)志1固定0xFE1負(fù)載長(zhǎng)度1Payload字節(jié)數(shù)2包序列號(hào)10-255循環(huán)3系統(tǒng)ID1發(fā)送設(shè)備系統(tǒng)編號(hào)4組件ID1發(fā)送設(shè)備組件編號(hào)5消息ID1消息類型6~N負(fù)載數(shù)據(jù)變化具體消息字段N1~N2校驗(yàn)和2CRC16/MCRF4XXV2版本在V1基礎(chǔ)上擴(kuò)展了幀頭起始標(biāo)志變成了0xFD并在消息ID區(qū)域從1字節(jié)擴(kuò)展到了3字節(jié)同時(shí)增加了額外的增量校驗(yàn)字節(jié)Increment/Compatibility flags。V2的幀頭是這樣的字節(jié)偏移字段名長(zhǎng)度說明0起始標(biāo)志1固定0xFD1負(fù)載長(zhǎng)度1Payload字節(jié)數(shù)2包序列號(hào)10-255循環(huán)3系統(tǒng)ID1發(fā)送設(shè)備系統(tǒng)編號(hào)4組件ID1發(fā)送設(shè)備組件編號(hào)5消息ID低字節(jié)1消息ID bit0-76消息ID中間字節(jié)1消息ID bit8-157消息ID高字節(jié)1消息ID bit16-238兼容性標(biāo)志1保留位9兼容性掩碼1保留位10~N9負(fù)載數(shù)據(jù)變化具體消息字段N10~N11校驗(yàn)和2CRC16/MCRF4XX這兩者的核心區(qū)別一個(gè)是消息ID的寬度V2最多支持16777216種消息類型而V1只有256種。另一個(gè)是V2增加了兩個(gè)標(biāo)志字節(jié)方便后續(xù)協(xié)議演進(jìn)。實(shí)際開發(fā)中V1和V2經(jīng)常會(huì)混用所以解析器必須同時(shí)兼容0xFE和0xFD兩種起始標(biāo)志。2.2 系統(tǒng)ID和組件ID到底怎么理解我剛接觸MAVLink時(shí)對(duì)系統(tǒng)ID和組件ID的概念有點(diǎn)模糊后來調(diào)鏈路時(shí)踩了坑才徹底明白。簡(jiǎn)單說系統(tǒng)ID用于區(qū)分同一通信鏈路上的不同飛行器或地面設(shè)備比如你同時(shí)控制3臺(tái)無人機(jī)那3臺(tái)飛機(jī)的系統(tǒng)ID一般會(huì)設(shè)成1、2、3。組件ID則用于區(qū)分同一個(gè)系統(tǒng)內(nèi)部的不同功能模塊比如自動(dòng)飛控組件、GPS模塊、相機(jī)云臺(tái)、地面站軟件它們的組件ID都不同。地面站和飛控通信時(shí)飛控發(fā)出的數(shù)據(jù)里系統(tǒng)ID是飛控自己的編號(hào)組件ID是各個(gè)板載組件的編號(hào)。而地面站要發(fā)送指令給飛控時(shí)發(fā)送方的系統(tǒng)ID必須和飛控配置中認(rèn)定的地面站系統(tǒng)ID一致否則飛控會(huì)直接忽略。這部分看起來不難但做多機(jī)場(chǎng)景時(shí)經(jīng)常有人把系統(tǒng)ID搞混導(dǎo)致指令發(fā)給1號(hào)機(jī)結(jié)果2號(hào)機(jī)沒有反應(yīng)。2.3 CRC_EXTRA很多人忽略卻致命的校驗(yàn)字節(jié)MAVLink的校驗(yàn)和計(jì)算方式跟普通CRC不太一樣它并不是單純對(duì)幀頭加負(fù)載做CRC16。實(shí)際算法是把整條消息幀的數(shù)據(jù)幀頭部分按特定規(guī)則參與不同版本參與的數(shù)據(jù)范圍不同和一個(gè)消息專屬的CRC_EXTRA種子值一起做CRC16/MCRF4XX運(yùn)算。這個(gè)CRC_EXTRA值是從消息定義XML中提取的具體數(shù)值與消息的字段順序、字段類型強(qiáng)相關(guān)。如果發(fā)送端和接收端對(duì)同一消息ID的字段定義不完全一致計(jì)算出的CRC_EXTRA就不一樣接收端理所當(dāng)然會(huì)把這條消息視為非法。以前和第三方設(shè)備做聯(lián)調(diào)時(shí)對(duì)方聲稱支持MAVLink結(jié)果發(fā)過來的消息我們這邊全部校驗(yàn)失敗最后排查發(fā)現(xiàn)對(duì)方的消息定義里把某個(gè)uint32_t字段改成了uint16_t導(dǎo)致CRC_EXTRA種子對(duì)不上。所以在手寫封包和解析時(shí)CRC_EXTRA值必須和你的目標(biāo)飛控固件所用的消息定義嚴(yán)格一致。ArduPilot和PX4的Common.xml定義可以從官方倉庫獲取改動(dòng)任何字段后都要重新計(jì)算CRC_EXTRA。3. 自己寫封包校驗(yàn)計(jì)算、字節(jié)序與代碼實(shí)現(xiàn)3.1 封包前必須先確認(rèn)的兩個(gè)參數(shù)目標(biāo)系統(tǒng)的字節(jié)序與CRC種子動(dòng)手寫封包代碼前先把兩個(gè)參數(shù)確認(rèn)好否則后面調(diào)試會(huì)非常痛苦。第一個(gè)是字節(jié)序。MAVLink協(xié)議規(guī)定所有多字節(jié)數(shù)值類型如uint16_t、uint32_t、float等在負(fù)載區(qū)都使用小端序Little-Endian存儲(chǔ)。這個(gè)規(guī)定幾乎所有飛控實(shí)現(xiàn)都遵循但如果是對(duì)接某些非標(biāo)準(zhǔn)的定制設(shè)備最好先確認(rèn)一下對(duì)方是否符合標(biāo)準(zhǔn)因?yàn)樽止?jié)序錯(cuò)了的話解析出來的數(shù)值會(huì)非常離譜比如高度變成幾百萬米。第二個(gè)是CRC種子。每個(gè)消息ID都對(duì)應(yīng)一個(gè)固定的CRC_EXTRA值這個(gè)值可以通過官方腳本crc_extra.py或在線工具查也可以從MAVLink XML消息定義中提取。我習(xí)慣把用到的消息ID和CRC_EXTRA值維護(hù)在一個(gè)常量類里作為封包和解析共用的對(duì)照表。3.2 手寫封包的核心流程封包的本質(zhì)就是把消息對(duì)象的字段按定義順序?qū)懭胍粋€(gè)字節(jié)緩沖區(qū)計(jì)算CRC再加上幀頭最后輸出完整的字節(jié)數(shù)組。以發(fā)送V1版本HEARTBEAT消息為例我的實(shí)現(xiàn)思路大致如下創(chuàng)建一個(gè)ByteBuffer設(shè)置為小端序如果字節(jié)序不匹配這里就要先調(diào)整否則后面所有數(shù)值字段都會(huì)出問題。預(yù)留幀頭部分的空間或者先把幀頭字段逐個(gè)寫入。由于負(fù)載長(zhǎng)度是先知的所以可以先把負(fù)載長(zhǎng)度、包序列號(hào)、系統(tǒng)ID、組件ID、消息ID依次寫入緩沖區(qū)。接下來寫入負(fù)載數(shù)據(jù)也就是消息具體字段。幀頭加負(fù)載全部寫入后用這份完整數(shù)據(jù)計(jì)算CRC16校驗(yàn)和再把2字節(jié)校驗(yàn)和追加到緩沖區(qū)末尾。最終輸出byte[]就是一條可發(fā)送的MAVLink消息幀。下面是一個(gè)偏基礎(chǔ)的實(shí)現(xiàn)示例目標(biāo)是可讀性優(yōu)先方便大家理解協(xié)議流程實(shí)際工程中可以根據(jù)需要再優(yōu)化。import java.nio.ByteBuffer; import java.nio.ByteOrder; public class MavlinkV1Frame { // 每個(gè)消息ID對(duì)應(yīng)的CRC_EXTRA值這里用HEARTBEAT舉例 private static final int CRC_EXTRA_HEARTBEAT 50; public static byte[] buildHeartbeatV1(int sysId, int compId, int seq) { // HEARTBEAT V1 負(fù)載長(zhǎng)度9 字節(jié) int payloadLength 9; ByteBuffer buf ByteBuffer.allocate(payloadLength 8); buf.order(ByteOrder.LITTLE_ENDIAN); buf.put((byte) 0xFE); // 起始標(biāo)志 buf.put((byte) payloadLength); // 負(fù)載長(zhǎng)度 buf.put((byte) seq); // 包序列號(hào) buf.put((byte) sysId); // 系統(tǒng)ID buf.put((byte) compId); // 組件ID buf.put((byte) 0); // 消息ID: HEARTBEAT // payload: MAV_TYPE_FIXED_WING1, MAV_AUTOPILOT_ARDUPILOTMEGA3 buf.put((byte) 1); // type buf.put((byte) 3); // autopilot buf.put((byte) 0); // base_mode buf.put((byte) 0); // custom_mode 低字節(jié) buf.put((byte) 0); // custom_mode 高字節(jié) buf.put((byte) 0); // system_status buf.put((byte) 0); // mavlink_version byte[] frame buf.array(); // 計(jì)算CRC16幀頭 負(fù)載并追加CRC_EXTRA種子參與運(yùn)算 int crc Crc16Util.crc16(frame, frame.length - 2); crc Crc16Util.accumulateByte(crc, CRC_EXTRA_HEARTBEAT); ByteBuffer result ByteBuffer.allocate(frame.length 2); result.order(ByteOrder.LITTLE_ENDIAN); result.put(frame); result.putShort((short) (crc 0xFFFF)); // 校驗(yàn)和低字節(jié)在前 return result.array(); } }這里的Crc16Util就是CRC16/MCRF4XX算法的具體實(shí)現(xiàn)。關(guān)于算法本身下文會(huì)詳細(xì)展開這里先留一個(gè)引用。3.3 CRC16/MCRF4XX怎么用Java實(shí)現(xiàn)MAVLink用的CRC16是CRC16/MCRF4XX它的多項(xiàng)式是0x1021初始值是0xFFFF。算法對(duì)輸入數(shù)據(jù)的每一位進(jìn)行處理和普通CRC16區(qū)別不大關(guān)鍵是MAVLink協(xié)議規(guī)定CRC結(jié)束后還需要把CRC_EXTRA種子當(dāng)作一個(gè)獨(dú)立字節(jié)繼續(xù)參與一次或多次累加運(yùn)算。以下是Java實(shí)現(xiàn)可以直接復(fù)用public class Crc16Util { public static int crc16(byte[] data) { int crc 0xFFFF; for (byte b : data) { crc ^ (b 0xFF) 8; for (int i 0; i 8; i) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } } return crc; } public static int crc16(byte[] data, int length) { int crc 0xFFFF; for (int i 0; i length; i) { crc ^ (data[i] 0xFF) 8; for (int j 0; j 8; j) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } } return crc; } public static int accumulateByte(int crc, int byteValue) { crc ^ (byteValue 0xFF) 8; for (int j 0; j 8; j) { if ((crc 0x8000) ! 0) { crc (crc 1) ^ 0x1021; } else { crc 1; } crc 0xFFFF; } return crc; } }我看到有些網(wǎng)上流傳的代碼在累加CRC_EXTRA時(shí)會(huì)忽略字節(jié)的高位其實(shí)那是不嚴(yán)格的。accumulateByte方法里必須把傳入值按0xFF取低位再放到高8位參與運(yùn)算這樣才能和官方實(shí)現(xiàn)保持一致。3.4 字節(jié)填充順序與ByteBuffer的選擇寫封包代碼時(shí)我最常犯的錯(cuò)誤是字段順序和官方消息定義不一致。MAVLink消息的負(fù)載字段順序不是隨意排的而是XML定義里從上到下的順序。比如GLOBAL_POSITION_INT的定義順序是time_boot_ms、lat、lon、alt、relative_alt、vx、vy、vz、hdg如果你把lat和lon寫反了經(jīng)緯度就會(huì)在圖上走出奇怪的軌跡。Java里處理二進(jìn)制流我推薦用ByteBuffer而不是手工拼接byte數(shù)組。原因有三點(diǎn)一是ByteBuffer原生支持小端序轉(zhuǎn)換只要order(ByteOrder.LITTLE_ENDIAN)設(shè)好putInt、putFloat這些方法會(huì)自動(dòng)處理字節(jié)順序二是它支持批量操作可以避免大量數(shù)組復(fù)制三是調(diào)試時(shí)可以直接看position和remaining方便定位寫入位置。不過要注意ByteBuffer的get/put操作會(huì)把內(nèi)部position向前移動(dòng)讀取時(shí)需要有意識(shí)的flip或使用絕對(duì)位置訪問。3.5 V2封包的差異點(diǎn)如果你要跟PX4或較新的ArduPilot通信V2封包更常見。V2和V1的封包流程差別不大但有幾個(gè)點(diǎn)要特別注意起始標(biāo)志是0xFD接收方如果只認(rèn)0xFE需要兩個(gè)分支同時(shí)兼容。消息ID是3字節(jié)寫入時(shí)按低、中、高的順序依次put。在幀頭多出的兩個(gè)標(biāo)志字節(jié)通常填0表示沒有開啟額外的簽名或兼容性擴(kuò)展。如果開啟了消息簽名功能幀尾除了2字節(jié)CRC還會(huì)追加13字節(jié)的簽名數(shù)據(jù)簽名機(jī)制比較復(fù)雜一般用不到但對(duì)接某些安全要求較高的系統(tǒng)時(shí)可能要處理。在實(shí)際工程里我建議優(yōu)先支持V2發(fā)送同時(shí)兼容V1解析因?yàn)榇蠖鄶?shù)現(xiàn)代飛控都兼容V2接收而很多舊設(shè)備還在發(fā)V1。4. 自己寫解析流式數(shù)據(jù)的狀態(tài)機(jī)處理4.1 粘包、半包與起始標(biāo)志誤判寫解析器時(shí)第一個(gè)要面對(duì)的問題就是MAVLink是流式協(xié)議底層可能是串口、TCP或UDP數(shù)據(jù)到達(dá)時(shí)不會(huì)剛好是一條完整的消息邊界。你可能會(huì)一次性收到好幾條消息粘在一起也可能收到一條消息的前半截過一會(huì)才收到后半截。最簡(jiǎn)單的處理思路就是一個(gè)字節(jié)一個(gè)字節(jié)地喂進(jìn)狀態(tài)機(jī)每完整收齊一幀就丟出一幀。這種方式雖然看起來笨拙但勝在穩(wěn)定不容易丟包而且實(shí)現(xiàn)邏輯直觀。效率上對(duì)于無人機(jī)數(shù)據(jù)鏈路這種吞吐量完全夠用。4.2 狀態(tài)機(jī)設(shè)計(jì)我把解析狀態(tài)機(jī)劃分成以下狀態(tài)WAITING_FOR_START等待起始標(biāo)志0xFE或0xFD。READING_HEADER正在讀取幀頭剩余字段根據(jù)起始標(biāo)志區(qū)分V1和V2的幀頭長(zhǎng)度。READING_PAYLOAD按幀頭聲明的負(fù)載長(zhǎng)度讀取負(fù)載數(shù)據(jù)。CHECKING_CRC讀取2字節(jié)校驗(yàn)和并計(jì)算比對(duì)?；具壿嬍莗ublic class MavlinkParser { private static final byte START_V1 (byte) 0xFE; private static final byte START_V2 (byte) 0xFD; private int state 0; // 0 等起始, 1 讀幀頭, 2 讀負(fù)載, 3 讀CRC private boolean isV2 false; private int headerLen 0; private int payloadLen 0; private int crcExtra 0; private byte[] header new byte[12]; private int headerPos 0; private byte[] payload new byte[255]; private int payloadPos 0; private int seq 0; private int sysId 0; private int compId 0; private int msgId 0; public void feed(byte b, ListMavlinkMessage out) { switch (state) { case 0: if (b START_V1) { isV2 false; headerLen 6; headerPos 0; state 1; } else if (b START_V2) { isV2 true; headerLen 10; headerPos 0; state 1; } break; case 1: header[headerPos] b; if (headerPos 1) { payloadLen b 0xFF; } if (headerPos headerLen) { // 解析幀頭字段 if (isV2) { seq header[2] 0xFF; sysId header[3] 0xFF; compId header[4] 0xFF; msgId (header[5] 0xFF) | ((header[6] 0xFF) 8) | ((header[7] 0xFF) 16); } else { seq header[2] 0xFF; sysId header[3] 0xFF; compId header[4] 0xFF; msgId header[5] 0xFF; } payloadPos 0; state 2; } break; case 2: if (payloadPos payloadLen) { payload[payloadPos] b; } if (payloadPos payloadLen) { state 3; } break; case 3: // 先讀到的低字節(jié)后讀到的高字節(jié) int crcLow b 0xFF; // 這里簡(jiǎn)化處理實(shí)際還需要第二個(gè)字節(jié)才能算完 // 建議把CRC作為一個(gè)兩字節(jié)小狀態(tài)再細(xì)分 break; } } }上面的代碼只是一個(gè)骨架實(shí)際用的時(shí)候需要把CRC校驗(yàn)部分拆成兩步先收crcLow再收crcHigh收齊后計(jì)算整幀數(shù)據(jù)的CRC然后把crcLow和crcHigh拼成int和計(jì)算值比對(duì)。為了方便閱讀我沒有把CRC子狀態(tài)寫進(jìn)去但思路是一致的。4.3 校驗(yàn)失敗怎么辦CRC校驗(yàn)失敗時(shí)最簡(jiǎn)單粗暴的做法就是把當(dāng)前這一幀丟棄然后回到WAITING_FOR_START狀態(tài)重新開始。這個(gè)策略在大多數(shù)字鏈路場(chǎng)景下是合適的因?yàn)镸AVLink本身沒有幀級(jí)重傳機(jī)制漏掉一幀姿態(tài)數(shù)據(jù)下一幀馬上就會(huì)補(bǔ)上實(shí)時(shí)性數(shù)據(jù)對(duì)單幀丟失并不敏感。但有一種情況要特別注意如果在等待CRC的連續(xù)兩個(gè)字節(jié)期間因?yàn)榇诓ㄌ芈逝渲貌粚?duì)或者鏈路干擾造成字節(jié)錯(cuò)位那么狀態(tài)機(jī)可能會(huì)進(jìn)入假同步。比如數(shù)據(jù)里恰好出現(xiàn)0xFE被你當(dāng)成起始標(biāo)志隨后幾字節(jié)又恰好滿足幀頭結(jié)構(gòu)結(jié)果組裝出一條校驗(yàn)失敗的假消息。處理辦法是CRC校驗(yàn)失敗后不要簡(jiǎn)單地把丟棄點(diǎn)之后的第一個(gè)0xFE當(dāng)作新起始而是應(yīng)該退回到起始標(biāo)志狀態(tài)同時(shí)保留當(dāng)前緩沖區(qū)里已經(jīng)讀過的數(shù)據(jù)從下一位繼續(xù)掃描。否則某些干擾環(huán)境下會(huì)出現(xiàn)頻繁丟真幀的現(xiàn)象。4.4 如何從原始字節(jié)流切出完整幀半包場(chǎng)景實(shí)測(cè)假設(shè)你從串口讀到了這樣一個(gè)字節(jié)流FE 09 01 01 01 00 ...后面還缺了20個(gè)字節(jié)才夠完整。這時(shí)狀態(tài)機(jī)會(huì)停在READING_PAYLOAD或CHECKING_CRC狀態(tài)。等下一批數(shù)據(jù)到達(dá)你要把新數(shù)據(jù)繼續(xù)喂給狀態(tài)機(jī)而不是重新開一幀。這就是狀態(tài)機(jī)的好處幀狀態(tài)跨批次保持天然處理了半包。反之如果一次讀到了三幀粘在一起狀態(tài)機(jī)也會(huì)在解析完第一幀后立刻把后續(xù)的0xFE識(shí)別為新幀的起始繼續(xù)解析第二幀、第三幀。整個(gè)過程不需要任何復(fù)雜緩沖區(qū)分割一個(gè)狀態(tài)機(jī)加一個(gè)回調(diào)接口就夠。在實(shí)際項(xiàng)目中我通常會(huì)在狀態(tài)機(jī)外面包一個(gè)parse(byte[] data)方法循環(huán)調(diào)用feed并把解析出來的完整消息收集到List返回。這樣一個(gè)方法就能統(tǒng)一處理單包多包、半包全包的全部場(chǎng)景。5. 工程化接入使用官方Java庫構(gòu)建數(shù)據(jù)交互鏈路5.1 用庫不等于不學(xué)協(xié)議自己手寫解析器非常適合學(xué)習(xí)協(xié)議但工程效率方面如果目標(biāo)只是快速跑通業(yè)務(wù)我還是推薦用官方生態(tài)的Java庫。dronefleet的mavlink庫是業(yè)界相對(duì)活躍的Java MAVLink庫它通過代碼生成器從XML消息定義生成Java類消息類型全CRC_EXTRA也提前算好了省去自己維護(hù)一大堆常量。不過注意用庫之前最好還是把我前面講的那套幀結(jié)構(gòu)、校驗(yàn)機(jī)制理解清楚因?yàn)閹熘荒軒湍憬鉀Q編解碼問題它不負(fù)責(zé)處理串口或網(wǎng)絡(luò)傳輸。數(shù)據(jù)從串口讀出來后該面對(duì)的半包、粘包問題依然要自己面對(duì)。5.2 引入依賴并發(fā)送HEARTBEAT消息以Maven項(xiàng)目為例引入dronefleet mavlink庫dependency groupIdio.dronefleet.mavlink/groupId artifactIdmavlink-core/artifactId version1.1.0/version /dependency如果是ArduPilot再加一個(gè)dependency groupIdio.dronefleet.mavlink/groupId artifactIdmavlink-ardupilotmega/artifactId version1.1.0/version /dependency有了依賴后發(fā)送HEARTBEAT消息的代碼大致如下import io.dronefleet.mavlink.MavlinkConnection; import io.dronefleet.mavlink.MavlinkMessage; import io.dronefleet.mavlink.common.Heartbeat; import io.dronefleet.mavlink.common.MavAutopilot; import io.dronefleet.mavlink.common.MavState; import io.dronefleet.mavlink.common.MavType; import java.io.ByteArrayInputStream; import java.io.ByteArrayOutputStream; public class MavlinkSender { private MavlinkConnection connection; public MavlinkSender() { ByteArrayOutputStream out new ByteArrayOutputStream(); ByteArrayInputStream in new ByteArrayInputStream(new byte[0]); connection MavlinkConnection.create(in, out); } public byte[] buildHeartbeat() { Heartbeat heartbeat Heartbeat.builder() .type(MavType.MAV_TYPE_FIXED_WING) .autopilot(MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA) .baseMode(0) .customMode(0) .systemStatus(MavState.MAV_STATE_STANDBY) .mavlinkVersion(3) .build(); MavlinkMessage? msg MavlinkMessage.create(1, 1, heartbeat); connection.send(msg, msg.getOriginSystemId(), msg.getOriginComponentId()); try { return out.toByteArray(); } finally { out.reset(); } } }這段代碼的關(guān)鍵邏輯在MavlinkConnection.send方法內(nèi)部它幫你處理了消息幀的組裝、CRC_EXTRA的寫入、序列號(hào)的維護(hù)。getOriginSystemId和getOriginComponentId分別是消息對(duì)象攜帶的系統(tǒng)ID和組件ID需要根據(jù)你的設(shè)備編號(hào)設(shè)置。5.3 解析庫的常見處理方式用庫解析消息時(shí)流程一般是從串口或網(wǎng)絡(luò)讀取原始字節(jié)。調(diào)用MavlinkConnection的next()方法傳入原始字節(jié)流返回解析出的MavlinkMessage對(duì)象。對(duì)返回的消息對(duì)象做類型匹配判斷是HEARTBEAT還是GLOBAL_POSITION_INT再?gòu)?qiáng)轉(zhuǎn)成具體類型取字段。一個(gè)簡(jiǎn)單的解析循環(huán)如下import io.dronefleet.mavlink.MavlinkConnection; import io.dronefleet.mavlink.MavlinkMessage; import io.dronefleet.mavlink.common.GlobalPositionInt; import io.dronefleet.mavlink.common.Heartbeat; import java.io.ByteArrayInputStream; public class MavlinkReceiver { public void handle(byte[] data) { ByteArrayInputStream in new ByteArrayInputStream(data); MavlinkConnection connection MavlinkConnection.create(in, new ByteArrayOutputStream()); // 注意這里只處理單幀實(shí)際場(chǎng)景需要循環(huán)調(diào)用直到數(shù)據(jù)耗盡 MavlinkMessage? msg connection.next(); if (msg.getMessage() instanceof Heartbeat) { Heartbeat hb (Heartbeat) msg.getMessage(); System.out.println(收到心跳類型: hb.type()); } else if (msg.getMessage() instanceof GlobalPositionInt) { GlobalPositionInt gpi (GlobalPositionInt) msg.getMessage(); System.out.println(緯度: gpi.lat() / 1.0E7); System.out.println(經(jīng)度: gpi.lon() / 1.0E7); System.out.println(高度: gpi.alt() / 1000.0); } } }這里的lat和lon是int類型單位是1e7度alt單位是毫米換算成常用單位時(shí)記得除以對(duì)應(yīng)系數(shù)。用庫雖然方便但協(xié)議層面的單位換算還是得自己心里有數(shù)。5.4 串口、TCP、UDP傳輸層怎么選MAVLink本身是獨(dú)立的協(xié)議層底層傳輸可以用UART串口、TCP或UDP。串口場(chǎng)景飛控通過數(shù)傳模塊連到地面站或工控機(jī)數(shù)據(jù)以字節(jié)流方式到達(dá)通常用jserialcomm或串口框架讀取。串口沒有消息邊界必須靠狀態(tài)機(jī)處理。TCP場(chǎng)景常見于數(shù)傳模塊通過TCP轉(zhuǎn)發(fā)數(shù)據(jù)或者機(jī)載計(jì)算機(jī)和地面站之間走TCP長(zhǎng)連接。TCP自帶流控和重傳但依然存在粘包問題處理方式和串口一樣。UDP場(chǎng)景常用于局域網(wǎng)內(nèi)的數(shù)據(jù)分發(fā)比如QGroundControl和多個(gè)地面終端之間的組播轉(zhuǎn)發(fā)。UDP雖然是數(shù)據(jù)報(bào)協(xié)議但MAVLink經(jīng)常把多條消息塞進(jìn)同一個(gè)UDP包所以依然需要按幀切分。無論底層的Transport是什么核心編解碼邏輯都可以復(fù)用同一套狀態(tài)機(jī)或同一套MavlinkConnection。我在工程里一般把傳輸層抽象成一個(gè)Transport接口只提供read和write兩個(gè)方法這樣切換串口和UDP就非常輕量。6. 實(shí)際調(diào)試中踩過的坑與定位方法6.1 坑一CRC_EXTRA值對(duì)不上消息被靜默丟棄有一次聯(lián)調(diào)對(duì)方用自研設(shè)備發(fā)HEARTBEAT我們用解析器接收解析器始終打印“CRC校驗(yàn)失敗”。查看抓包數(shù)據(jù)幀頭、負(fù)載、長(zhǎng)度都完全正確但CRC就是不對(duì)。反復(fù)核對(duì)后才發(fā)現(xiàn)對(duì)方在生成消息定義時(shí)把system_status字段從uint8_t改成了uint16_t。字段類型變化導(dǎo)致負(fù)載長(zhǎng)度和CRC_EXTRA都變了幀頭里寫的負(fù)載長(zhǎng)度是新長(zhǎng)度但CRC_EXTRA用的是舊值整個(gè)校驗(yàn)自然失敗。這個(gè)坑教會(huì)我一件事CRC_EXTRA不只是一個(gè)消息ID對(duì)應(yīng)的常量它本身就是消息定義的指紋。任何字段順序、類型、長(zhǎng)度的變化都會(huì)改變它。所以聯(lián)調(diào)時(shí)如果CRC瘋狂失敗先不要懷疑算法實(shí)現(xiàn)先核對(duì)雙方的消息定義XML是否完全一致。這比一行行看代碼高效得多。6.2 坑二串口讀數(shù)時(shí)buffer不夠?qū)е聨財(cái)嘤么谧x數(shù)時(shí)如果讀取緩沖區(qū)設(shè)置得比較小可能出現(xiàn)一次讀到的數(shù)據(jù)只是一幀消息的一半另一半還留在串口緩沖區(qū)里。如果讀取代碼寫得比較簡(jiǎn)單比如讀一次就嘗試解析那就會(huì)出現(xiàn)大量半包導(dǎo)致的解析失敗。我的處理方案是讀到的每一批數(shù)據(jù)都先追加到一個(gè)公共的緩沖隊(duì)列然后讓狀態(tài)機(jī)持續(xù)消費(fèi)緩沖隊(duì)列里的數(shù)據(jù)直到隊(duì)列為空。不要每讀到一批數(shù)據(jù)就做一次“起點(diǎn)尋找完整校驗(yàn)”那是錯(cuò)誤姿勢(shì)。6.3 坑三非小端序造成的臟數(shù)據(jù)Java在x86平臺(tái)上默認(rèn)是大端序執(zhí)行ByteBuffer.wrap(bytes).getInt()讀出的數(shù)據(jù)在MAVLink場(chǎng)景里往往會(huì)得到一個(gè)很大的數(shù)。很多新手在里踩了坑之后以為是自己解析錯(cuò)位了其實(shí)只是忘了設(shè)置ByteOrder.LITTLE_ENDIAN。在涉及float字段時(shí)這個(gè)坑更隱蔽。比如高度、速度、姿態(tài)角這些字段很多是float類型如果字節(jié)序不對(duì)解析出來的數(shù)值看起來不是邏輯錯(cuò)誤而是一些非常奇怪的小數(shù)。遇到這種數(shù)值不要懷疑人生先檢查ByteOrder。6.4 坑四消息序列號(hào)與丟包統(tǒng)計(jì)MAVLink幀頭里的包序列號(hào)seq可以用來統(tǒng)計(jì)鏈路丟包率。我在做鏈路質(zhì)量檢測(cè)時(shí)會(huì)記錄每個(gè)系統(tǒng)ID組件ID組合的最近一次序列號(hào)如果下一次收到的序列號(hào)與期望值不連續(xù)就說明中間有丟幀。這個(gè)功能看著簡(jiǎn)單但沒有序列號(hào)意識(shí)的人往往忽略它而它在實(shí)飛調(diào)試中非常有用能快速判斷是數(shù)傳鏈路問題還是解析程序問題。6.5 坑五坐標(biāo)系與單位換算MAVLink里的經(jīng)緯度使用的是整數(shù)類型單位是1e7度也就是說lat373774819代表緯度37.3774819度。高度字段可能是毫米空速可能是厘米每秒地速可能是厘米每秒角速度可能是厘度每秒。不同消息的字段單位差異巨大尤其是從飛控切換到PX4再到ArduPilot時(shí)某些擴(kuò)展消息的單位還會(huì)變化。我在做數(shù)據(jù)展示時(shí)會(huì)把單位換算集中到一個(gè)轉(zhuǎn)換層不散落到各個(gè)業(yè)務(wù)方法里這樣即使后續(xù)協(xié)議單位發(fā)生變化只需要改轉(zhuǎn)換層一處。7. 用仿真環(huán)境驗(yàn)證整條鏈路7.1 搭建一個(gè)本地MAVLink驗(yàn)證環(huán)境代碼寫完了怎么驗(yàn)證封包和解析是否正確我的習(xí)慣是先用MAVProxy或者QGroundControl連接一個(gè)SITL仿真飛控這樣不炸機(jī)也能把整個(gè)鏈路跑通。SITL是ArduPilot的軟件在環(huán)仿真可以在PC上虛擬出一臺(tái)完整飛控。啟動(dòng)后SITL會(huì)在本地UDP端口14550上輸出MAVLinkQGroundControl默認(rèn)也能連這個(gè)端口。如果你只想驗(yàn)證自己寫的Java解析器可以讓Java程序監(jiān)聽UDP 14550端口把收到的UDP數(shù)據(jù)包交給狀態(tài)機(jī)解析打印出HEARTBEAT和GLOBAL_POSITION_INT信息。如果程序能持續(xù)打印出正常的經(jīng)緯度和高度說明解析鏈路是通的。7.2 發(fā)送航點(diǎn)消息給仿真飛控的驗(yàn)證步驟如果你想驗(yàn)證封包能力比如發(fā)送MissionItem消息給飛控也可以直接用SITL來驗(yàn)證。流程是啟動(dòng)SITL仿真。用你的Java程序向SITL的UDP端口發(fā)送MISSION_COUNT、MISSION_ITEM最后發(fā)送MISSION_START等消息。在QGroundControl地面站上觀察航點(diǎn)是否被接收并可以執(zhí)行。要注意的是ArduPilot對(duì)航點(diǎn)消息有一套握手流程不是直接發(fā)一個(gè)MISSION_ITEM就能完成的。你需要按順序處理MISSION_REQUEST_LIST等往返消息否則飛控不會(huì)接受航點(diǎn)。很多人第一次做航點(diǎn)上傳時(shí)在這里卡了很久建議直接看官方MAVLink的Mission Protocol狀態(tài)機(jī)圖搞清楚每個(gè)狀態(tài)該發(fā)什么消息。7.3 自定義消息擴(kuò)展的思路如果你有特殊的數(shù)據(jù)需要走M(jìn)AVLink比如機(jī)載AI識(shí)別結(jié)果、私有傳感器數(shù)據(jù)正規(guī)做法是自定義MAVLink消息擴(kuò)展。思路是在common.xml或ardupilotmega.xml中新增消息定義分配一個(gè)未使用的消息ID。運(yùn)行代碼生成器重新生成Java類。用新的Java類編解碼自定義消息。整個(gè)過程不算簡(jiǎn)單但可行性很高。ArduPilot固件側(cè)也可以注冊(cè)對(duì)應(yīng)的自定義消息處理邏輯。如果你不想改飛控固件還可以使用MAVLink的MAV_CMD、NAMED_VALUE_FLOAT、DEBUG_VECT等通用消息來傳輸自定義數(shù)據(jù)這些消息在常見飛控固件里都有支持不需要改動(dòng)固件。8. 調(diào)試工具與效率提升建議8.1 抓包工具比打印日志好用得多解析MAVLink時(shí)遇到詭異問題我強(qiáng)烈建議先用抓包工具看原始二進(jìn)制而不是在代碼里瘋狂打印日志。可以在電腦上用Wireshark抓UDP或串口數(shù)據(jù)包再用4243端口或串口監(jiān)控工具把二進(jìn)制流導(dǎo)出成hex文件。hex文件一看起始標(biāo)志、幀長(zhǎng)度、消息ID是否連續(xù)、CRC是否規(guī)律性失敗基本一目了然。Java程序里也可以加一個(gè)可開關(guān)的hex dump工具類在高頻調(diào)試階段打開把每個(gè)UDP包或串口讀取到的數(shù)據(jù)以hex格式輸出。聯(lián)調(diào)時(shí)讓對(duì)方也打開同樣的hex dump兩邊一對(duì)比是發(fā)送端的問題還是接收端的問題立刻清楚。8.2 消息日志要帶序列號(hào)和時(shí)間戳日志不能只打“收到HEARTBEAT”這種信息最好把幀頭的seq、系統(tǒng)ID、組件ID、CRC校驗(yàn)結(jié)果一起打出來。seq和時(shí)間戳能幫你定位丟幀規(guī)律系統(tǒng)ID和組件ID能幫你區(qū)分多設(shè)備混連時(shí)的消息來源。這些信息在后續(xù)排查多機(jī)干擾、鏈路偶發(fā)斷連時(shí)都是關(guān)鍵線索。8.3 建立消息ID對(duì)照表如果你接觸的MAVLink消息比較多建議在項(xiàng)目里維護(hù)一個(gè)消息ID和消息名稱的對(duì)照表這樣抓包看到msgId33就能立刻反應(yīng)過來是GLOBAL_POSITION_INT而不是每次都要臨時(shí)查XML定義。這個(gè)表也可以用于日志過濾比如只查看姿態(tài)消息或只查看心跳消息。9. 從單機(jī)到多機(jī)數(shù)據(jù)交互設(shè)計(jì)的一些體會(huì)實(shí)際做多機(jī)場(chǎng)景時(shí)MAVLink的編解碼本身不是最復(fù)雜的部分復(fù)雜的是如何從流式數(shù)據(jù)中區(qū)分不同飛行器的消息。前面說過每架飛機(jī)的系統(tǒng)ID不同所以在解析出消息后第一件事就是根據(jù)源系統(tǒng)ID做路由。我的做法是維護(hù)一個(gè)MapInteger, DeviceSessionkey是系統(tǒng)IDvalue是當(dāng)前在線設(shè)備的狀態(tài)快照。每次解析出一條消息就更新對(duì)應(yīng)設(shè)備的狀態(tài)快照同時(shí)記錄最后活躍時(shí)間。這樣一個(gè)極簡(jiǎn)的設(shè)備管理服務(wù)就能支撐上百臺(tái)無人機(jī)的狀態(tài)監(jiān)控。心跳超時(shí)檢測(cè)也建議做一下。MAVLink的HEARTBEAT默認(rèn)1Hz發(fā)送如果連續(xù)3到5秒沒有收到某個(gè)系統(tǒng)ID的心跳基本可以判定該設(shè)備鏈路中斷。我在服務(wù)里用定時(shí)任務(wù)掃描設(shè)備會(huì)話的最后活躍時(shí)間超過閾值就標(biāo)記離線并推送告警。另外多機(jī)同時(shí)操作時(shí)指令消息必須攜帶正確的目標(biāo)系統(tǒng)ID和目標(biāo)組件ID。MAVLink的大多數(shù)指令消息都有target_system和target_component字段如果填錯(cuò)飛控可能會(huì)忽略指令。真實(shí)項(xiàng)目中因?yàn)閠arget_system填錯(cuò)導(dǎo)致指令發(fā)給另一臺(tái)飛機(jī)的教訓(xùn)太多了所以發(fā)送前一定要確認(rèn)目標(biāo)編號(hào)。從協(xié)議解析到封包發(fā)送從單機(jī)調(diào)試到多機(jī)管理MAVLink這套體系本身并不復(fù)雜但它藏了很多細(xì)節(jié)任何一個(gè)環(huán)節(jié)出問題整條數(shù)據(jù)鏈路都會(huì)變得不可用。希望我上面這些代碼和踩坑經(jīng)驗(yàn)?zāi)軒湍闵僮咭恍澛?。如果你正在做相關(guān)的Java項(xiàng)目遇到具體的協(xié)議細(xì)節(jié)問題也歡迎交流。