議收發(fā)源碼實戰(zhàn)：幀結構、CRC校驗與聯(lián)調經驗)
簡介MAVLink協(xié)議收發(fā)源碼是一套基于C的輕量級通信協(xié)議實現(xiàn)面向無人機、機器人控制及地面站開發(fā)者重點解決設備之間數(shù)據(jù)高效編解碼與串口透傳問題。壓縮包共663個文件以606個h頭文件為主體涵蓋MAVLink標準協(xié)議定義、消息類型與編解碼接口另有cpp示例、vcxproj/sln工程配置和可執(zhí)行demo等整體約57.69MB。目前已有2374人學習瀏覽。資料中的demo示例演示了從串口初始化、構造消息、字節(jié)流序列化到接收解析與CRC校驗的完整收發(fā)流程調用mavlink_msg_to_send_buffer和mavlink_parse_char即可快速搭建通信鏈路進而按消息ID處理GPS坐標、傳感器讀數(shù)等數(shù)據(jù)。對于需要自主實現(xiàn)飛控/地面站通信模塊或深入閱讀MAVLink協(xié)議的C開發(fā)者來說這是一份貼近工程實踐的參考源碼。 做嵌入式或者無人機開發(fā)的朋友大概率都繞不開一個詞mavlink協(xié)議收發(fā)源碼。不管是給飛控寫機載計算機的通信程序還是自己搭一套地面站又或者只是想在STM32上報個姿態(tài)數(shù)據(jù)最終都會落到“怎么把MAVLink消息正確發(fā)出去、再把對方的消息收進來并解析對”這個底層問題上。這篇我就把實際項目中調通MAVLink協(xié)議收發(fā)的完整經驗拆開講從幀結構、CRC校驗到收發(fā)代碼怎么組織再到聯(lián)調現(xiàn)場踩過的那些坑一次說透。適合正在搞飛控二次開發(fā)、打算自己寫通信模塊、或者被協(xié)議棧源碼搞得頭大的朋友直接參考。1. 為什么無人機通信離不開MAVLink1.1 從一根串口的血案說起先說一個我早年間遇到的真實場景。當時做一個基于Pixhawk的植保無人機項目機載電腦需要實時讀取飛控的姿態(tài)和GPS數(shù)據(jù)同時要能下發(fā)航線任務。最開始圖省事直接按照飛控官方文檔里某個帖子的格式自己定義了一套串口報文幀頭用0xAA第二個字節(jié)是長度后面跟著數(shù)據(jù)最后來一個累加和。結果一上真機就出問題。地面站軟件Mission Planner能正常連飛控我的程序卻經常解析出亂碼偶爾出來一個正確姿態(tài)值下一幀又是錯的。排查了整整兩天才意識到飛控默認往外吐的是MAVLink而不是我自定義的格式而且Port速率、協(xié)議版本這些都會影響實際收到的字節(jié)流。說白了我是在和一個我根本不了解的協(xié)議較勁。后來老老實實把MAVLink的協(xié)議規(guī)范看了一遍才發(fā)現(xiàn)這個協(xié)議之所以能成為無人機領域的事實標準不是沒有理由的。MAVLink全稱Micro Air Vehicle Link最初就是為微型飛行器設計的點對點通信協(xié)議現(xiàn)在已經被ArduPilot、PX4、QGroundControl、Mission Planner這些主流軟硬件廣泛采用。它的設計目標很直接在帶寬受限的串口鏈路上把飛控的各種消息心跳、姿態(tài)、GPS、遙測、命令可靠地打包傳輸。1.2 MAVLink的設計哲學工具鏈生態(tài)是第一生產力很多初學者不理解為什么放著簡單的自定義協(xié)議不用非得用一個看起來規(guī)則繁瑣的MAVLink。我的理解是它解決的根本不是“能不能傳數(shù)據(jù)”的問題而是“整個生態(tài)怎么高效協(xié)作”的問題。MAVLink定義了一整套標準消息集HEARTBEAT心跳、ATTITUDE姿態(tài)、GPS_RAW_INTGPS原始數(shù)據(jù)、MISSION_ITEM任務點等等。這意味著你寫的地面站不需要知道飛控是PX4還是ArduPilot只要雙方都遵循MAVLink規(guī)范就能互相理解。更關鍵的是它配套了完整的工具鏈xml消息定義文件、代碼生成器mavgen、各語言庫C/C、Python等。你改一個xml就能自動生成對應語言的收發(fā)代碼不需要手動維護一套龐大的消息結構體。這種“協(xié)議規(guī)范代碼生成生態(tài)工具”三位一體的思路是MAVLink最值錢的地方。所以當你決定寫一個mavlink協(xié)議收發(fā)源碼時本質上不是從零發(fā)明協(xié)議而是把這條生態(tài)鏈路接入到自己的項目里。2. 幀格式與CRC_EXTRA收發(fā)源碼的兩個硬門檻2.1 幀結構逐字節(jié)拆解要寫收發(fā)代碼第一關就是把幀格式吃透。很多網上流傳的源碼寫得不清楚就是因為在幀格式的理解上偷了懶。MAVLink目前有兩個大版本v1和v2實際項目中遇到的基本都是v2但v1的老設備也還在服役收發(fā)代碼最好能兼容兩者。MAVLink v2的一幀數(shù)據(jù)長這樣字節(jié)位置字段名長度說明0STX1幀起始標志固定為0xFD1LEN1負載長度0~2552INC_FLAGS1不兼容標志必須為0否則接收方可能拒絕3CMP_FLAGS1兼容標志通常為04SEQ1消息序列號用于丟包檢測5SYS_ID1系統(tǒng)ID區(qū)分不同飛行器/地面站6COMP_ID1組件ID區(qū)分同一系統(tǒng)內的不同模塊7~9MSG_ID3消息ID小端序排列10~PAYLOAD0~255消息負載末尾-2CKA1校驗和高字節(jié)末尾-1CKB1校驗和低字節(jié)v1的幀則簡單一些沒有INC_FLAGS和CMP_FLAGSMSG_ID只有1字節(jié)起始標志是0xFE。收發(fā)程序里解析幀頭時要先根據(jù)STX判斷是哪個版本再按對應的格式讀取。這里有一個特別容易踩的細節(jié)MSG_ID在v2里是3字節(jié)而且是小端序。比如消息ID 0x000100在幀里出現(xiàn)的是00 01 00不是00 00 01。很多自研源碼收不到消息問題就出在這個字節(jié)序上。2.2 CRC_EXTRA協(xié)議給收發(fā)雙方設的暗號幀格式里有CRC校驗這本身不稀奇。但MAVLink特殊的地方在于在計算CRC的時候除了幀里的字段和負載還要額外疊加一個叫CRC_EXTRA的種子字節(jié)。這個字節(jié)由消息ID和消息定義共同決定每條消息都不一樣。為什么這么設計你可以把CRC_EXTRA理解成“收發(fā)雙方約定的暗號”。它保證了發(fā)送方和接收方必須對消息定義各個字段的類型、長度、排列順序保持一致。如果有一方用的協(xié)議版本或者消息定義和另一方對不上即使幀格式完全正確算出來的CRC也一定不一致接收方就會丟掉這幀。這比普通的CRC校驗更能防止“能解析但解析錯”的隱性bug。CRC_EXTRA不是一個固定值它由MAVLink的代碼生成器在生成頭文件時計算好。如果你用的是官方生成的C/C庫這個值已經包含在一個靜態(tài)數(shù)組里了如果你自己寫解析源碼就需要從生成的代碼或者pymavlink庫里把對應消息的CRC_EXTRA摳出來。這個值的具體算法是先把消息定義按規(guī)則序列化對每個字段做一次CRC更新X.25算法初始0xFFFF多項式0x1021最終得到一個字節(jié)。這個算法本身不復雜但沒有必要自己重新實現(xiàn)直接用官方工具生成的表就行。我在實際代碼里是直接把CRC_EXTRA數(shù)組定義成了常量表和消息ID一一對應。2.3 字節(jié)序、消息ID寬度與會話簽名的邊界情況收發(fā)了多輪MAVLink之后我整理出幾個幀級細節(jié)新手特別容易栽進去第一除了MSG_ID其余多字節(jié)字段也是小端序。比如GPS_RAW_INT中的緯度、經度都是int32在負載里的字節(jié)序是低字節(jié)在前解析時如果用memcpy直接轉結構體在沒有做字節(jié)序處理的平臺或者做跨平臺移植時會出現(xiàn)數(shù)值完全對不上的現(xiàn)象。第二v2幀的INC_FLAGS如果不為0接收方可能要檢查簽名。MAVLink v2支持一套基于密鑰的簽名機制在INC_FLAGS的第0位置1時幀尾會額外追加13字節(jié)的簽名信息。自研收發(fā)代碼時如果不想處理簽名接收端可以直接把帶簽名的幀丟棄或者設置兼容標志讓發(fā)送方不啟用簽名。更省事的做法是自己組包時INC_FLAGS永遠置0不給自己找麻煩。第三SEQ序列號是用來檢測丟包和幀亂序的。發(fā)送端每發(fā)一幀就自增10~255循環(huán)接收端可以通過相鄰兩幀SEQ的差值判斷中間是否丟幀。如果做數(shù)據(jù)鏈路層質量評估這個字段是免費的統(tǒng)計素材。3. 收發(fā)實現(xiàn)的三條路線怎么選不后悔3.1 官方C庫完整但厚重寫mavlink協(xié)議收發(fā)源碼最常見的方案是直接用官方C語言庫。從GitHub上拉取c_library_v2里面按消息集分了目錄直接把需要的那幾個頭文件拷進項目調用mavlink_msg_heartbeat_pack、mavlink_msg_heartbeat_decode這類函數(shù)就能完成收發(fā)。這個方案的優(yōu)點是不用自己維護消息結構體和CRC表支持所有消息天然適配v2和簽名機制。缺點也很明顯代碼體積巨大動輒幾千行對MCU的Flash和RAM都有壓力另一個問題是封裝的層次太多出了問題不好排查。比如我曾在某個國產MCU上遇到CRC總是算不對最后發(fā)現(xiàn)是編譯器默認的char類型是無符號還是帶符號影響了移位運算的結果這種問題在官方庫里排查起來非常繞。所以官方C庫適合兩種場景一是跑在資源充足的機載電腦上二是需要全量消息支持的完整系統(tǒng)。如果只是某個資源緊巴巴的STM32項目用它可能有點殺雞用牛刀。3.2 pymavlink調試和仿真階段的最佳輔助pymavlink是MAVLink生態(tài)里最靈活的Python庫。它既能作為地面站的通信后端也能當作調試工具來用。我經常在串口調試階段直接用pymavlink寫個小腳本往飛控發(fā)一條命令看飛控返回什么驗證我的自研收發(fā)源碼是不是真的把幀發(fā)對了。比如下面這段代碼就是典型的pymavlink姿勢from pymavlink import mavutil master mavutil.mavlink_connection(COM7, baud57600) master.wait_heartbeat() print(收到心跳系統(tǒng)ID:, master.target_system) # 請求飛控發(fā)送姿態(tài)數(shù)據(jù) master.mav.command_long_send( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0, mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 200000, 0, 0, 0, 0, 0 ) while True: msg master.recv_match(typeATTITUDE, blockingTrue) print(msg.roll, msg.pitch, msg.yaw)pymavlink的價值在于它把MAVLink的編解碼、CRC_EXTRA計算、消息定義全部封裝好了你可以用它來生成參考數(shù)據(jù)再拿這些數(shù)據(jù)去驗證自己的C代碼。比如抓一幀pymavlink發(fā)出的原始字節(jié)對比自己的封幀函數(shù)輸出很快就能定位是幀頭錯了、長度錯了還是CRC算錯了。3.3 自研精簡版資源受限MCU的真正解法如果你的目標平臺Flash只有64KBRAM只有16KB或者你和我一樣需要在一個特殊的國產芯片上跑MAVLink那自研一套精簡收發(fā)源碼就是繞不開的路。這個“自研”不是完全從零手寫所有消息而是只保留你需要的消息子集比如心跳、姿態(tài)、GPS可能再加一兩個命令控制消息然后用自己的代碼實現(xiàn)幀組幀解析和CRC校驗。自研代碼的核心工程量有三個部分幀結構定義、CRC計算含CRC_EXTRA表、收發(fā)狀態(tài)機。下面一節(jié)我就把這部分的代碼邏輯完整拆開。4. 自研MAVLink收發(fā)器的核心源碼拆解4.1 幀結構體與CRC_EXTRA表先把基礎的數(shù)據(jù)結構定義好。我的做法是直接把幀頭和負載放連續(xù)緩沖區(qū)里構造這樣算CRC和發(fā)送都非常方便。以下是一份以STM32為背景、但移植性很強的C語言實現(xiàn)骨架。#define MAVLINK_STX_V2 0xFD #define MAVLINK_STX_V1 0xFE #define MAVLINK_IFLAG_SIGNED 0x01 typedef struct { uint8_t stx; uint8_t len; uint8_t inc_flags; uint8_t cmp_flags; uint8_t seq; uint8_t sysid; uint8_t compid; uint32_t msgid; // 實際占3字節(jié)小端 uint8_t payload[255]; uint8_t cka; uint8_t ckb; } mavlink_frame_t;接收時我傾向于先把整幀收進一個緩沖區(qū)校驗通過后再解析而不是邊收邊解析。這樣邏輯清晰也方便處理半幀和粘幀的情況。CRC_EXTRA表直接從官方生成的頭文件里抄過來只保留會用到的消息。例如static const uint8_t mavlink_crc_extra[] { [0] 50, // HEARTBEAT [30] 39, // ATTITUDE [24] 23, // GPS_RAW_INT [74] 103, // VFR_HUD // ... };注意不同版本的消息ID相同但CRC_EXTRA不一定相同最好從和你實際使用的協(xié)議版本配套的生成文件里提取不要拿網上隨意找的老表直接套用。我在一個舊項目里就吃過這個虧表是1.0版本的協(xié)議棧是2.0結果所有消息校驗不過排查了很久才發(fā)現(xiàn)是版本錯配。4.2 發(fā)送鏈路封幀、組包、算校驗發(fā)送一條消息邏輯上就是把負載塞到幀里、填好幀頭、算出CRC然后從串口打出去。以發(fā)送心跳消息為例HEARTBEAT的負載是7個字節(jié)type(1)、autopilot(1)、base_mode(1)、custom_mode(4)、system_status(1)、mavlink_version(1)。void mavlink_send_heartbeat(uint8_t sysid, uint8_t compid, uint8_t seq) { uint8_t buf[MAVLINK_MAX_FRAME_LEN]; uint16_t crc 0xFFFF; uint8_t payload[7]; payload[0] MAV_TYPE_QUADROTOR; // type payload[1] MAV_AUTOPILOT_ARDUPILOTMEGA; // autopilot payload[2] 0; // base_mode payload[3] 0; // custom_mode low byte payload[4] 0; payload[5] 0; payload[6] 0; // custom_mode high byte payload[7] MAV_STATE_ACTIVE; // system_status payload[8] 3; // mavlink_version // 幀頭 buf[0] MAVLINK_STX_V2; buf[1] sizeof(payload); buf[2] 0; // inc_flags buf[3] 0; // cmp_flags buf[4] seq; buf[5] sysid; buf[6] compid; buf[7] 0x00; // msgid low buf[8] 0x00; // msgid mid buf[9] 0x00; // msgid high memcpy(buf[10], payload, sizeof(payload)); // CRC計算從LEN字段開始到負載結束再追加CRC_EXTRA crc crc_calculate(buf[1], 9 sizeof(payload)); crc crc_accumulate(mavlink_crc_extra[MAVLINK_MSG_ID_HEARTBEAT], crc); buf[10 sizeof(payload)] (crc 8) 0xFF; buf[11 sizeof(payload)] crc 0xFF; uart_send(buf, 12 sizeof(payload)); }CRC算法是X.25初值0xFFFF多項式0x1021實現(xiàn)代碼網上很多但有一點必須強調CRC計算的起點是LEN字段不包括STX。這是MAVLink規(guī)范和普通幀校驗不一樣的地方很多自己寫的源碼發(fā)出去的幀接收方總是回NACK原因往往就在這里。4.3 接收鏈路串口中斷DMA與狀態(tài)機解析接收端是mavlink協(xié)議收發(fā)源碼里最容易出bug的部分。我的建議是串口中斷或者DMA只負責把原始字節(jié)塞進環(huán)形緩沖區(qū)絕對不要在中斷里做CRC計算或者消息解析主循環(huán)里統(tǒng)一做狀態(tài)機解析這樣能避免中斷嵌套導致的狀態(tài)錯亂。解析狀態(tài)機可以用一個簡單的枚舉來描述typedef enum { PARSE_STX, PARSE_LEN, PARSE_REST_HEADER, PARSE_PAYLOAD, PARSE_CRC } parse_state_t;核心邏輯是一個逐字節(jié)的switch-case循環(huán)。每來一個字節(jié)當前狀態(tài)決定怎么處理在PARSE_STX狀態(tài)遇到0xFD就判定為v2幀并進入PARSE_LEN遇到0xFE則按v1處理如果是其他字節(jié)說明鏈路里可能有噪聲繼續(xù)等待。在PARSE_LEN狀態(tài)記錄負載長度。如果負載長度超過預設最大值比如255說明幀頭可能錯亂直接回到PARSE_STX重新同步。在PARSE_REST_HEADER狀態(tài)把剩余幀頭字段讀齊。這里要注意v2的MSG_ID是3字節(jié)要按小端拼成一個uint32。在PARSE_PAYLOAD狀態(tài)收滿len個負載字節(jié)后進入PARSE_CRC。在PARSE_CRC狀態(tài)收齊2字節(jié)校驗碼對整個幀從LEN開始計算CRC并與收到的校驗碼比對相同則投遞到消息處理隊列然后無論結果如何都回到PARSE_STX。int mavlink_parse_byte(uint8_t c, mavlink_frame_t *frame, parse_state_t *state, uint8_t *payload_buf, uint16_t *payload_idx) { switch (*state) { case PARSE_STX: if (c MAVLINK_STX_V2) { frame-stx c; *state PARSE_LEN; } else if (c MAVLINK_STX_V1) { // v1處理len在第二個字節(jié)沒有inc/cmp frame-stx c; *state PARSE_LEN; } // 忽略其他雜散字節(jié)保持同步等待 break; case PARSE_LEN: frame-len c; if (frame-len 255) { *state PARSE_STX; // 長度異常重新同步 return -1; } *payload_idx 0; *state PARSE_REST_HEADER; break; case PARSE_REST_HEADER: // 按幀格式依次讀取后續(xù)字段v1與v2路徑不同 // 這里以v2為例 // ... break; case PARSE_PAYLOAD: payload_buf[(*payload_idx)] c; if (*payload_idx frame-len) { *state PARSE_CRC; } break; case PARSE_CRC: // 收齊兩個校驗字節(jié)后做完整校驗 // ... break; } return 0; }這里有一個實操經驗接收緩沖區(qū)至少要能存放2倍最大幀長否則高波特率下主循環(huán)處理不過來環(huán)形緩沖會互相覆蓋。比如波特率921600一幀最長280字節(jié)左右緩沖區(qū)建議直接上512字節(jié)或更大。另一個經驗是如果鏈路噪聲比較嚴重會出現(xiàn)大量解析失敗的分組此時不要簡單地丟棄就算了最好在調試階段把丟掉的幀字節(jié)數(shù)通過調試串口打出來幫助判斷是物理層的問題還是協(xié)議層的問題。5. 聯(lián)調實錄三個典型故障的完整排查鏈路5.1 地面站收不到心跳緩沖區(qū)管理問題現(xiàn)象很典型自研飛控板連接QGroundControl地面站一直提示“no heartbeat received”但用串口助手看飛控的TX引腳數(shù)據(jù)明明在往外吐。排查鏈路我先從物理層開始用示波器看TX波形確認波特率沒有配置錯也排除了TTL電平和RS232電平不匹配的干擾。接著用邏輯分析儀直接抓串口數(shù)據(jù)發(fā)現(xiàn)飛控發(fā)的第一幀看起來是0xFD開頭這是對的。再把抓到的二進制流導出用pymavlink的解析腳本去解析結果顯示CRC錯誤。這就很奇怪了因為幀頭幀尾初始都正常CRC怎么會錯后來我把目光放到環(huán)形緩沖區(qū)上才發(fā)現(xiàn)問題接收側DMA沒有開啟半傳輸中斷當緩沖區(qū)寫指針超過頂端后數(shù)據(jù)會覆蓋未讀的數(shù)據(jù)。在高負載時覆蓋發(fā)生在主循環(huán)還沒來得及取出數(shù)據(jù)的時候等主循環(huán)去取時讀到的是被篡改過的幀自然CRC不過。修復方案是把環(huán)形緩沖區(qū)的讀指針和寫指針都做原子操作保護并且給DMA開半傳輸中斷和傳輸完成中斷保證在緩沖區(qū)滿之前就把數(shù)據(jù)搬運出去。改完后再跑心跳穩(wěn)定輸出問題解決。5.2 CRC校驗狂失敗EXTRA表錯位問題另一個項目里接收pymavlink發(fā)來的命令我的自研代碼幾乎一條命令都解析不了每條都在CRC校驗階段被丟棄。最開始懷疑是時序問題把波特率從57600降到9600也沒用。后來我在代碼里加了一個調試信息打印出收到的CRC和本地算出的CRC。一對照發(fā)現(xiàn)收到的CRC總是變而本地算出的CRC總是和官方庫不一樣差值不固定說明問題不是簡單的常數(shù)偏移。這時我想到了CRC_EXTRA表如果表的索引和消息ID不對應算出來的CRC會隨機地時而正確、時而錯誤因為不同消息的CRC_EXTRA值各不相同。進一步檢查果然發(fā)現(xiàn)我的CRC_EXTRA數(shù)組是從網絡上一篇老文章里復制的里面按照v1時代的消息索引排的序。v2的MSG_ID改為3字節(jié)后很多消息的索引已經變了。我把數(shù)組改為直接從mavgen生成的v2頭文件里提取重新編譯后CRC全部通過。這個坑的教訓是CRC_EXTRA表必須和協(xié)議版本、消息定義嚴格綁定不要抄網上流傳的“通用表”。5.3 飛控能收不能發(fā)載荷長度與消息ID不匹配還有一種情況是接收正常發(fā)送卻一直被對方忽略。用QGroundControl連接飛控時飛控能正常收到地面站發(fā)來的命令但地面站一直收不到飛控的姿態(tài)數(shù)據(jù)。排查鏈路先排除CRC問題——因為CRC計算和接收用的是同一套表如果接收正常CRC本身應該沒問題。再檢查消息ID發(fā)現(xiàn)我發(fā)送姿態(tài)消息時往消息ID里填的是HEARTBEAT的ID而負載卻是ATTITUDE的內容。這個錯誤在協(xié)議層很容易被忽略因為MAVLink接收端往往只按消息ID去匹配解析邏輯不會檢查負載內容是否合理。結果就是地面站收到了ID為30的消息按ATTITUDE格式解析得出一個奇怪的角度值干脆把它當無效數(shù)據(jù)丟棄。這也引出一個設計經驗消息ID和負載的定義必須一致不僅在代碼層面最好在規(guī)劃文檔里也理清楚。發(fā)消息前先做一次“發(fā)送拜拜測試”用pymavlink收到這幀后把msg.get_type()答應出來確認ID和負載匹配再往上層用。6. 一個親測有效的小技巧最后分享一個我在多個項目里驗證過的小技巧拿到一套新的MAVLink收發(fā)源碼無論它來自官方、開源社區(qū)還是自己寫的先不要直接接到飛控上聯(lián)調。第一步是把它插到電腦上用pymavlink跑一個回環(huán)測試——把自研代碼的發(fā)送引腳和pymavlink的接收引腳短接反之亦然然后用pymavlink發(fā)一幀標準數(shù)據(jù)觀察自己的代碼能否正確解析再讓自己的代碼發(fā)一幀用pymavlink解析。這樣可以在不依賴飛控、不受真實鏈路干擾的情況下把協(xié)議層的收發(fā)正確性驗證得明明白白。等這一步全部通過再上飛控聯(lián)調你會發(fā)現(xiàn)自己省下了至少一個下午的排錯時間。MAVLink這東西協(xié)議本身并不難難的是細節(jié)而這些細節(jié)幾乎都是可以提前在桌面上就排查掉的。本文還有配套的精品資源點擊獲取 MAVLink協(xié)議收發(fā)源碼實戰(zhàn)：幀結構、CRC校驗與聯(lián)調經驗
簡介MAVLink協(xié)議收發(fā)源碼是一套基于C的輕量級通信協(xié)議實現(xiàn)面向無人機、機器人控制及地面站開發(fā)者重點解決設備之間數(shù)據(jù)高效編解碼與串口透傳問題。壓縮包共663個文件以606個h頭文件為主體涵蓋MAVLink標準協(xié)議定義、消息類型與編解碼接口另有cpp示例、vcxproj/sln工程配置和可執(zhí)行demo等整體約57.69MB。目前已有2374人學習瀏覽。資料中的demo示例演示了從串口初始化、構造消息、字節(jié)流序列化到接收解析與CRC校驗的完整收發(fā)流程調用mavlink_msg_to_send_buffer和mavlink_parse_char即可快速搭建通信鏈路進而按消息ID處理GPS坐標、傳感器讀數(shù)等數(shù)據(jù)。對于需要自主實現(xiàn)飛控/地面站通信模塊或深入閱讀MAVLink協(xié)議的C開發(fā)者來說這是一份貼近工程實踐的參考源碼。 做嵌入式或者無人機開發(fā)的朋友大概率都繞不開一個詞mavlink協(xié)議收發(fā)源碼。不管是給飛控寫機載計算機的通信程序還是自己搭一套地面站又或者只是想在STM32上報個姿態(tài)數(shù)據(jù)最終都會落到“怎么把MAVLink消息正確發(fā)出去、再把對方的消息收進來并解析對”這個底層問題上。這篇我就把實際項目中調通MAVLink協(xié)議收發(fā)的完整經驗拆開講從幀結構、CRC校驗到收發(fā)代碼怎么組織再到聯(lián)調現(xiàn)場踩過的那些坑一次說透。適合正在搞飛控二次開發(fā)、打算自己寫通信模塊、或者被協(xié)議棧源碼搞得頭大的朋友直接參考。1. 為什么無人機通信離不開MAVLink1.1 從一根串口的血案說起先說一個我早年間遇到的真實場景。當時做一個基于Pixhawk的植保無人機項目機載電腦需要實時讀取飛控的姿態(tài)和GPS數(shù)據(jù)同時要能下發(fā)航線任務。最開始圖省事直接按照飛控官方文檔里某個帖子的格式自己定義了一套串口報文幀頭用0xAA第二個字節(jié)是長度后面跟著數(shù)據(jù)最后來一個累加和。結果一上真機就出問題。地面站軟件Mission Planner能正常連飛控我的程序卻經常解析出亂碼偶爾出來一個正確姿態(tài)值下一幀又是錯的。排查了整整兩天才意識到飛控默認往外吐的是MAVLink而不是我自定義的格式而且Port速率、協(xié)議版本這些都會影響實際收到的字節(jié)流。說白了我是在和一個我根本不了解的協(xié)議較勁。后來老老實實把MAVLink的協(xié)議規(guī)范看了一遍才發(fā)現(xiàn)這個協(xié)議之所以能成為無人機領域的事實標準不是沒有理由的。MAVLink全稱Micro Air Vehicle Link最初就是為微型飛行器設計的點對點通信協(xié)議現(xiàn)在已經被ArduPilot、PX4、QGroundControl、Mission Planner這些主流軟硬件廣泛采用。它的設計目標很直接在帶寬受限的串口鏈路上把飛控的各種消息心跳、姿態(tài)、GPS、遙測、命令可靠地打包傳輸。1.2 MAVLink的設計哲學工具鏈生態(tài)是第一生產力很多初學者不理解為什么放著簡單的自定義協(xié)議不用非得用一個看起來規(guī)則繁瑣的MAVLink。我的理解是它解決的根本不是“能不能傳數(shù)據(jù)”的問題而是“整個生態(tài)怎么高效協(xié)作”的問題。MAVLink定義了一整套標準消息集HEARTBEAT心跳、ATTITUDE姿態(tài)、GPS_RAW_INTGPS原始數(shù)據(jù)、MISSION_ITEM任務點等等。這意味著你寫的地面站不需要知道飛控是PX4還是ArduPilot只要雙方都遵循MAVLink規(guī)范就能互相理解。更關鍵的是它配套了完整的工具鏈xml消息定義文件、代碼生成器mavgen、各語言庫C/C、Python等。你改一個xml就能自動生成對應語言的收發(fā)代碼不需要手動維護一套龐大的消息結構體。這種“協(xié)議規(guī)范代碼生成生態(tài)工具”三位一體的思路是MAVLink最值錢的地方。所以當你決定寫一個mavlink協(xié)議收發(fā)源碼時本質上不是從零發(fā)明協(xié)議而是把這條生態(tài)鏈路接入到自己的項目里。2. 幀格式與CRC_EXTRA收發(fā)源碼的兩個硬門檻2.1 幀結構逐字節(jié)拆解要寫收發(fā)代碼第一關就是把幀格式吃透。很多網上流傳的源碼寫得不清楚就是因為在幀格式的理解上偷了懶。MAVLink目前有兩個大版本v1和v2實際項目中遇到的基本都是v2但v1的老設備也還在服役收發(fā)代碼最好能兼容兩者。MAVLink v2的一幀數(shù)據(jù)長這樣字節(jié)位置字段名長度說明0STX1幀起始標志固定為0xFD1LEN1負載長度0~2552INC_FLAGS1不兼容標志必須為0否則接收方可能拒絕3CMP_FLAGS1兼容標志通常為04SEQ1消息序列號用于丟包檢測5SYS_ID1系統(tǒng)ID區(qū)分不同飛行器/地面站6COMP_ID1組件ID區(qū)分同一系統(tǒng)內的不同模塊7~9MSG_ID3消息ID小端序排列10~PAYLOAD0~255消息負載末尾-2CKA1校驗和高字節(jié)末尾-1CKB1校驗和低字節(jié)v1的幀則簡單一些沒有INC_FLAGS和CMP_FLAGSMSG_ID只有1字節(jié)起始標志是0xFE。收發(fā)程序里解析幀頭時要先根據(jù)STX判斷是哪個版本再按對應的格式讀取。這里有一個特別容易踩的細節(jié)MSG_ID在v2里是3字節(jié)而且是小端序。比如消息ID 0x000100在幀里出現(xiàn)的是00 01 00不是00 00 01。很多自研源碼收不到消息問題就出在這個字節(jié)序上。2.2 CRC_EXTRA協(xié)議給收發(fā)雙方設的暗號幀格式里有CRC校驗這本身不稀奇。但MAVLink特殊的地方在于在計算CRC的時候除了幀里的字段和負載還要額外疊加一個叫CRC_EXTRA的種子字節(jié)。這個字節(jié)由消息ID和消息定義共同決定每條消息都不一樣。為什么這么設計你可以把CRC_EXTRA理解成“收發(fā)雙方約定的暗號”。它保證了發(fā)送方和接收方必須對消息定義各個字段的類型、長度、排列順序保持一致。如果有一方用的協(xié)議版本或者消息定義和另一方對不上即使幀格式完全正確算出來的CRC也一定不一致接收方就會丟掉這幀。這比普通的CRC校驗更能防止“能解析但解析錯”的隱性bug。CRC_EXTRA不是一個固定值它由MAVLink的代碼生成器在生成頭文件時計算好。如果你用的是官方生成的C/C庫這個值已經包含在一個靜態(tài)數(shù)組里了如果你自己寫解析源碼就需要從生成的代碼或者pymavlink庫里把對應消息的CRC_EXTRA摳出來。這個值的具體算法是先把消息定義按規(guī)則序列化對每個字段做一次CRC更新X.25算法初始0xFFFF多項式0x1021最終得到一個字節(jié)。這個算法本身不復雜但沒有必要自己重新實現(xiàn)直接用官方工具生成的表就行。我在實際代碼里是直接把CRC_EXTRA數(shù)組定義成了常量表和消息ID一一對應。2.3 字節(jié)序、消息ID寬度與會話簽名的邊界情況收發(fā)了多輪MAVLink之后我整理出幾個幀級細節(jié)新手特別容易栽進去第一除了MSG_ID其余多字節(jié)字段也是小端序。比如GPS_RAW_INT中的緯度、經度都是int32在負載里的字節(jié)序是低字節(jié)在前解析時如果用memcpy直接轉結構體在沒有做字節(jié)序處理的平臺或者做跨平臺移植時會出現(xiàn)數(shù)值完全對不上的現(xiàn)象。第二v2幀的INC_FLAGS如果不為0接收方可能要檢查簽名。MAVLink v2支持一套基于密鑰的簽名機制在INC_FLAGS的第0位置1時幀尾會額外追加13字節(jié)的簽名信息。自研收發(fā)代碼時如果不想處理簽名接收端可以直接把帶簽名的幀丟棄或者設置兼容標志讓發(fā)送方不啟用簽名。更省事的做法是自己組包時INC_FLAGS永遠置0不給自己找麻煩。第三SEQ序列號是用來檢測丟包和幀亂序的。發(fā)送端每發(fā)一幀就自增10~255循環(huán)接收端可以通過相鄰兩幀SEQ的差值判斷中間是否丟幀。如果做數(shù)據(jù)鏈路層質量評估這個字段是免費的統(tǒng)計素材。3. 收發(fā)實現(xiàn)的三條路線怎么選不后悔3.1 官方C庫完整但厚重寫mavlink協(xié)議收發(fā)源碼最常見的方案是直接用官方C語言庫。從GitHub上拉取c_library_v2里面按消息集分了目錄直接把需要的那幾個頭文件拷進項目調用mavlink_msg_heartbeat_pack、mavlink_msg_heartbeat_decode這類函數(shù)就能完成收發(fā)。這個方案的優(yōu)點是不用自己維護消息結構體和CRC表支持所有消息天然適配v2和簽名機制。缺點也很明顯代碼體積巨大動輒幾千行對MCU的Flash和RAM都有壓力另一個問題是封裝的層次太多出了問題不好排查。比如我曾在某個國產MCU上遇到CRC總是算不對最后發(fā)現(xiàn)是編譯器默認的char類型是無符號還是帶符號影響了移位運算的結果這種問題在官方庫里排查起來非常繞。所以官方C庫適合兩種場景一是跑在資源充足的機載電腦上二是需要全量消息支持的完整系統(tǒng)。如果只是某個資源緊巴巴的STM32項目用它可能有點殺雞用牛刀。3.2 pymavlink調試和仿真階段的最佳輔助pymavlink是MAVLink生態(tài)里最靈活的Python庫。它既能作為地面站的通信后端也能當作調試工具來用。我經常在串口調試階段直接用pymavlink寫個小腳本往飛控發(fā)一條命令看飛控返回什么驗證我的自研收發(fā)源碼是不是真的把幀發(fā)對了。比如下面這段代碼就是典型的pymavlink姿勢from pymavlink import mavutil master mavutil.mavlink_connection(COM7, baud57600) master.wait_heartbeat() print(收到心跳系統(tǒng)ID:, master.target_system) # 請求飛控發(fā)送姿態(tài)數(shù)據(jù) master.mav.command_long_send( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0, mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 200000, 0, 0, 0, 0, 0 ) while True: msg master.recv_match(typeATTITUDE, blockingTrue) print(msg.roll, msg.pitch, msg.yaw)pymavlink的價值在于它把MAVLink的編解碼、CRC_EXTRA計算、消息定義全部封裝好了你可以用它來生成參考數(shù)據(jù)再拿這些數(shù)據(jù)去驗證自己的C代碼。比如抓一幀pymavlink發(fā)出的原始字節(jié)對比自己的封幀函數(shù)輸出很快就能定位是幀頭錯了、長度錯了還是CRC算錯了。3.3 自研精簡版資源受限MCU的真正解法如果你的目標平臺Flash只有64KBRAM只有16KB或者你和我一樣需要在一個特殊的國產芯片上跑MAVLink那自研一套精簡收發(fā)源碼就是繞不開的路。這個“自研”不是完全從零手寫所有消息而是只保留你需要的消息子集比如心跳、姿態(tài)、GPS可能再加一兩個命令控制消息然后用自己的代碼實現(xiàn)幀組幀解析和CRC校驗。自研代碼的核心工程量有三個部分幀結構定義、CRC計算含CRC_EXTRA表、收發(fā)狀態(tài)機。下面一節(jié)我就把這部分的代碼邏輯完整拆開。4. 自研MAVLink收發(fā)器的核心源碼拆解4.1 幀結構體與CRC_EXTRA表先把基礎的數(shù)據(jù)結構定義好。我的做法是直接把幀頭和負載放連續(xù)緩沖區(qū)里構造這樣算CRC和發(fā)送都非常方便。以下是一份以STM32為背景、但移植性很強的C語言實現(xiàn)骨架。#define MAVLINK_STX_V2 0xFD #define MAVLINK_STX_V1 0xFE #define MAVLINK_IFLAG_SIGNED 0x01 typedef struct { uint8_t stx; uint8_t len; uint8_t inc_flags; uint8_t cmp_flags; uint8_t seq; uint8_t sysid; uint8_t compid; uint32_t msgid; // 實際占3字節(jié)小端 uint8_t payload[255]; uint8_t cka; uint8_t ckb; } mavlink_frame_t;接收時我傾向于先把整幀收進一個緩沖區(qū)校驗通過后再解析而不是邊收邊解析。這樣邏輯清晰也方便處理半幀和粘幀的情況。CRC_EXTRA表直接從官方生成的頭文件里抄過來只保留會用到的消息。例如static const uint8_t mavlink_crc_extra[] { [0] 50, // HEARTBEAT [30] 39, // ATTITUDE [24] 23, // GPS_RAW_INT [74] 103, // VFR_HUD // ... };注意不同版本的消息ID相同但CRC_EXTRA不一定相同最好從和你實際使用的協(xié)議版本配套的生成文件里提取不要拿網上隨意找的老表直接套用。我在一個舊項目里就吃過這個虧表是1.0版本的協(xié)議棧是2.0結果所有消息校驗不過排查了很久才發(fā)現(xiàn)是版本錯配。4.2 發(fā)送鏈路封幀、組包、算校驗發(fā)送一條消息邏輯上就是把負載塞到幀里、填好幀頭、算出CRC然后從串口打出去。以發(fā)送心跳消息為例HEARTBEAT的負載是7個字節(jié)type(1)、autopilot(1)、base_mode(1)、custom_mode(4)、system_status(1)、mavlink_version(1)。void mavlink_send_heartbeat(uint8_t sysid, uint8_t compid, uint8_t seq) { uint8_t buf[MAVLINK_MAX_FRAME_LEN]; uint16_t crc 0xFFFF; uint8_t payload[7]; payload[0] MAV_TYPE_QUADROTOR; // type payload[1] MAV_AUTOPILOT_ARDUPILOTMEGA; // autopilot payload[2] 0; // base_mode payload[3] 0; // custom_mode low byte payload[4] 0; payload[5] 0; payload[6] 0; // custom_mode high byte payload[7] MAV_STATE_ACTIVE; // system_status payload[8] 3; // mavlink_version // 幀頭 buf[0] MAVLINK_STX_V2; buf[1] sizeof(payload); buf[2] 0; // inc_flags buf[3] 0; // cmp_flags buf[4] seq; buf[5] sysid; buf[6] compid; buf[7] 0x00; // msgid low buf[8] 0x00; // msgid mid buf[9] 0x00; // msgid high memcpy(buf[10], payload, sizeof(payload)); // CRC計算從LEN字段開始到負載結束再追加CRC_EXTRA crc crc_calculate(buf[1], 9 sizeof(payload)); crc crc_accumulate(mavlink_crc_extra[MAVLINK_MSG_ID_HEARTBEAT], crc); buf[10 sizeof(payload)] (crc 8) 0xFF; buf[11 sizeof(payload)] crc 0xFF; uart_send(buf, 12 sizeof(payload)); }CRC算法是X.25初值0xFFFF多項式0x1021實現(xiàn)代碼網上很多但有一點必須強調CRC計算的起點是LEN字段不包括STX。這是MAVLink規(guī)范和普通幀校驗不一樣的地方很多自己寫的源碼發(fā)出去的幀接收方總是回NACK原因往往就在這里。4.3 接收鏈路串口中斷DMA與狀態(tài)機解析接收端是mavlink協(xié)議收發(fā)源碼里最容易出bug的部分。我的建議是串口中斷或者DMA只負責把原始字節(jié)塞進環(huán)形緩沖區(qū)絕對不要在中斷里做CRC計算或者消息解析主循環(huán)里統(tǒng)一做狀態(tài)機解析這樣能避免中斷嵌套導致的狀態(tài)錯亂。解析狀態(tài)機可以用一個簡單的枚舉來描述typedef enum { PARSE_STX, PARSE_LEN, PARSE_REST_HEADER, PARSE_PAYLOAD, PARSE_CRC } parse_state_t;核心邏輯是一個逐字節(jié)的switch-case循環(huán)。每來一個字節(jié)當前狀態(tài)決定怎么處理在PARSE_STX狀態(tài)遇到0xFD就判定為v2幀并進入PARSE_LEN遇到0xFE則按v1處理如果是其他字節(jié)說明鏈路里可能有噪聲繼續(xù)等待。在PARSE_LEN狀態(tài)記錄負載長度。如果負載長度超過預設最大值比如255說明幀頭可能錯亂直接回到PARSE_STX重新同步。在PARSE_REST_HEADER狀態(tài)把剩余幀頭字段讀齊。這里要注意v2的MSG_ID是3字節(jié)要按小端拼成一個uint32。在PARSE_PAYLOAD狀態(tài)收滿len個負載字節(jié)后進入PARSE_CRC。在PARSE_CRC狀態(tài)收齊2字節(jié)校驗碼對整個幀從LEN開始計算CRC并與收到的校驗碼比對相同則投遞到消息處理隊列然后無論結果如何都回到PARSE_STX。int mavlink_parse_byte(uint8_t c, mavlink_frame_t *frame, parse_state_t *state, uint8_t *payload_buf, uint16_t *payload_idx) { switch (*state) { case PARSE_STX: if (c MAVLINK_STX_V2) { frame-stx c; *state PARSE_LEN; } else if (c MAVLINK_STX_V1) { // v1處理len在第二個字節(jié)沒有inc/cmp frame-stx c; *state PARSE_LEN; } // 忽略其他雜散字節(jié)保持同步等待 break; case PARSE_LEN: frame-len c; if (frame-len 255) { *state PARSE_STX; // 長度異常重新同步 return -1; } *payload_idx 0; *state PARSE_REST_HEADER; break; case PARSE_REST_HEADER: // 按幀格式依次讀取后續(xù)字段v1與v2路徑不同 // 這里以v2為例 // ... break; case PARSE_PAYLOAD: payload_buf[(*payload_idx)] c; if (*payload_idx frame-len) { *state PARSE_CRC; } break; case PARSE_CRC: // 收齊兩個校驗字節(jié)后做完整校驗 // ... break; } return 0; }這里有一個實操經驗接收緩沖區(qū)至少要能存放2倍最大幀長否則高波特率下主循環(huán)處理不過來環(huán)形緩沖會互相覆蓋。比如波特率921600一幀最長280字節(jié)左右緩沖區(qū)建議直接上512字節(jié)或更大。另一個經驗是如果鏈路噪聲比較嚴重會出現(xiàn)大量解析失敗的分組此時不要簡單地丟棄就算了最好在調試階段把丟掉的幀字節(jié)數(shù)通過調試串口打出來幫助判斷是物理層的問題還是協(xié)議層的問題。5. 聯(lián)調實錄三個典型故障的完整排查鏈路5.1 地面站收不到心跳緩沖區(qū)管理問題現(xiàn)象很典型自研飛控板連接QGroundControl地面站一直提示“no heartbeat received”但用串口助手看飛控的TX引腳數(shù)據(jù)明明在往外吐。排查鏈路我先從物理層開始用示波器看TX波形確認波特率沒有配置錯也排除了TTL電平和RS232電平不匹配的干擾。接著用邏輯分析儀直接抓串口數(shù)據(jù)發(fā)現(xiàn)飛控發(fā)的第一幀看起來是0xFD開頭這是對的。再把抓到的二進制流導出用pymavlink的解析腳本去解析結果顯示CRC錯誤。這就很奇怪了因為幀頭幀尾初始都正常CRC怎么會錯后來我把目光放到環(huán)形緩沖區(qū)上才發(fā)現(xiàn)問題接收側DMA沒有開啟半傳輸中斷當緩沖區(qū)寫指針超過頂端后數(shù)據(jù)會覆蓋未讀的數(shù)據(jù)。在高負載時覆蓋發(fā)生在主循環(huán)還沒來得及取出數(shù)據(jù)的時候等主循環(huán)去取時讀到的是被篡改過的幀自然CRC不過。修復方案是把環(huán)形緩沖區(qū)的讀指針和寫指針都做原子操作保護并且給DMA開半傳輸中斷和傳輸完成中斷保證在緩沖區(qū)滿之前就把數(shù)據(jù)搬運出去。改完后再跑心跳穩(wěn)定輸出問題解決。5.2 CRC校驗狂失敗EXTRA表錯位問題另一個項目里接收pymavlink發(fā)來的命令我的自研代碼幾乎一條命令都解析不了每條都在CRC校驗階段被丟棄。最開始懷疑是時序問題把波特率從57600降到9600也沒用。后來我在代碼里加了一個調試信息打印出收到的CRC和本地算出的CRC。一對照發(fā)現(xiàn)收到的CRC總是變而本地算出的CRC總是和官方庫不一樣差值不固定說明問題不是簡單的常數(shù)偏移。這時我想到了CRC_EXTRA表如果表的索引和消息ID不對應算出來的CRC會隨機地時而正確、時而錯誤因為不同消息的CRC_EXTRA值各不相同。進一步檢查果然發(fā)現(xiàn)我的CRC_EXTRA數(shù)組是從網絡上一篇老文章里復制的里面按照v1時代的消息索引排的序。v2的MSG_ID改為3字節(jié)后很多消息的索引已經變了。我把數(shù)組改為直接從mavgen生成的v2頭文件里提取重新編譯后CRC全部通過。這個坑的教訓是CRC_EXTRA表必須和協(xié)議版本、消息定義嚴格綁定不要抄網上流傳的“通用表”。5.3 飛控能收不能發(fā)載荷長度與消息ID不匹配還有一種情況是接收正常發(fā)送卻一直被對方忽略。用QGroundControl連接飛控時飛控能正常收到地面站發(fā)來的命令但地面站一直收不到飛控的姿態(tài)數(shù)據(jù)。排查鏈路先排除CRC問題——因為CRC計算和接收用的是同一套表如果接收正常CRC本身應該沒問題。再檢查消息ID發(fā)現(xiàn)我發(fā)送姿態(tài)消息時往消息ID里填的是HEARTBEAT的ID而負載卻是ATTITUDE的內容。這個錯誤在協(xié)議層很容易被忽略因為MAVLink接收端往往只按消息ID去匹配解析邏輯不會檢查負載內容是否合理。結果就是地面站收到了ID為30的消息按ATTITUDE格式解析得出一個奇怪的角度值干脆把它當無效數(shù)據(jù)丟棄。這也引出一個設計經驗消息ID和負載的定義必須一致不僅在代碼層面最好在規(guī)劃文檔里也理清楚。發(fā)消息前先做一次“發(fā)送拜拜測試”用pymavlink收到這幀后把msg.get_type()答應出來確認ID和負載匹配再往上層用。6. 一個親測有效的小技巧最后分享一個我在多個項目里驗證過的小技巧拿到一套新的MAVLink收發(fā)源碼無論它來自官方、開源社區(qū)還是自己寫的先不要直接接到飛控上聯(lián)調。第一步是把它插到電腦上用pymavlink跑一個回環(huán)測試——把自研代碼的發(fā)送引腳和pymavlink的接收引腳短接反之亦然然后用pymavlink發(fā)一幀標準數(shù)據(jù)觀察自己的代碼能否正確解析再讓自己的代碼發(fā)一幀用pymavlink解析。這樣可以在不依賴飛控、不受真實鏈路干擾的情況下把協(xié)議層的收發(fā)正確性驗證得明明白白。等這一步全部通過再上飛控聯(lián)調你會發(fā)現(xiàn)自己省下了至少一個下午的排錯時間。MAVLink這東西協(xié)議本身并不難難的是細節(jié)而這些細節(jié)幾乎都是可以提前在桌面上就排查掉的。本文還有配套的精品資源點擊獲取