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