# MAVLink 原理及開發指南 ## I. 範例程式及使用說明 > [GitHub](https://github.com/shengwen-tw/mavlink_example) ```bash= git clone https://github.com/shengwen-tw/mavlink_example cd mavlink_example/ make all ``` ### 1-1. 使用 USB-TTL 模組進行 Loopback 測試: 將 TXD, RXD 短路: ![](https://hackmd.io/_uploads/HkQyHmz23.jpg) 操作: ```bash= ./mavlink -v -s /dev/ttyUSBX -b 115200 ``` ### 1-2.作為 TCP/IP Client 連線: ```bash= ./mavlink -v -i ip-address -p port-number ``` `-n` 用於提示使用 TCP/IP 連線 `-v` 用於是否印出訊息提示接收到的封包。 `-s` 後接 USB-TTL 裝置的路徑 `-b` 後接序列埠的 Baudrate `-i` 後接 IP 位址 `-b` 後接網路 Port number (埠號) ## II. 使用 Codegen 產生 C 語言標頭檔 由於 MAVLink 使用 XML 定義並以 Codegen 產生程式碼，因此各個飛控專案/廠商可以逕行擴充。在產生程式碼前必須先取得目標平台的 XML 檔。基本上大型的 Open-source 專案皆可在 MAVLink 官方 [GitHub](https://github.com/mavlink/mavlink/tree/master/message_definitions/v1.0) 頁面找到。 以下給出 Codegen 產生程式碼的操作流程: ```bash= sudo apt-get install python3-lxml git clone https://github.com/mavlink/mavlink.git --recursive cd mavlink/ python3 ./pymavlink/tools/mavgen.py \ --lang=C --wire-protocol=2.0 \ --output=generated/include/mavlink/v2.0 \ message_definitions/v1.0/common.xml ``` 這裡我們指定了 `common.xml`，即 PX4 的封包定義。而 pymavlink 中的 `mavgen.py` 即是 Codegen 程式本體。另外這裡也提供預先產生好的[結果](https://github.com/Tenok-RTOS/mavlink)。 註: 下述的範例程式已包含了 MAVLink 程式碼，不需要再額外產生。 ## III. 基本原理 ### 3-1. MAVLink 封包格式 在 MAVLink 中無論是發送或是接收，都僅是操作以下的資料結構: ![](https://hackmd.io/_uploads/Hy-MU8M32.png) 以下幾個部分值得注意: * SYS ID: System ID，用於標註**發送者**，0 表示 Broadcasting, 一般使用 1-255。 * COMP ID: Component ID, 標示**發送者**具體是什麼元件，例如可以設定為 Autopilot 加上 Camera (見 [MAV_COMPONENT](https://mavlink.io/en/messages/common.html#MAV_COMPONENT)) * MSG ID: 封包的編號，例如 0 是代表 Heatbeat * Payload: 數據區塊 **Remark:** 透過檢查 SYS ID 和 COMP ID 可以確定封包的來源。 詳細說明可見: [Packet Serialization ](https://mavlink.io/en/guide/serialization.html) --- 範例程式使用了兩個執行緒處理 MAVLink 封包 (Message) 的收發。 ### 3-2. mavlink_rx_thread() * 不斷嘗試讀取新的 Byte 並傳入 `mavlink_parse_char()` 函式進行解碼 * 將解碼成功後的 `recvd_msg` 透過 Message queue 傳至 `mavlink_tx_thread()` 中做出回應 ```c= void *mavlink_rx_thread(void *arg) { uint8_t c; mavlink_status_t status; mavlink_message_t recvd_msg; while (1) { read(mavlink_fd, &c, 1); /* Try parsing the message with new income byte */ if (mavlink_parse_char(MAVLINK_COMM_1, c, &recvd_msg, &status) == 1) { /* Notify the tx thread */ write(msg_fifo_tx, &recvd_msg, sizeof(recvd_msg)); } } } ``` ### 3-3. mavlink_tx_thread() * 固定時間發送特定封包 (由 `MSG_SCHEDULER_INIT()` 和 `MSG_SEND_HZ()` 巨集函式控制) * 從 Message queue 取出在 `mavlink_rx_thread` 接收到 (解出) 的封包並進行回應。 * 回應的程式最簡單的只要回應 Ack 封包，較複雜的 (即 [Microservice](https://mavlink.io/en/services/)) 則要根據功能維護一個狀態機並進行合適的回覆。 * `parse_mavlink_msg()` 是用於根據接收到的 Message ID 觸發對應的 MAVLink Handler，若是跟 Microservice 有關則必須再改變對應的狀態機狀態。 * 狀態機改變後，Microservice handler 必須再做出回應。 * `mavlink_tx_thread()` 每 10ms 僅能觸發一次，用以限制 CPU 資源使用 (經驗上來說 100Hz 以足夠充分使用) ```c= void *mavlink_tx_thread(void *arg) { MSG_SCHEDULER_INIT(1); // 1Hz mavlink_message_t recvd_msg; while (1) { /* clang-format off */ MSG_SEND_HZ(1, mavlink_send_play_tune(); mavlink_send_heartbeat(); ); /* clang-format on */ /* Trigger the command parser if received new message from the queue */ if (read(msg_fifo_rx, &recvd_msg, sizeof(recvd_msg)) == sizeof(recvd_msg)) { parse_mavlink_msg(&recvd_msg); } /* Limit CPU usage of the thread with execution frequency of 100Hz */ usleep(10000); // 10000us = 10ms } } ``` ### 3-4. parse_mavlink_msg() * 在 `mavlink_parser.c` 中新增 MAVLink handler function 用於回應接收到的封包 * 例: ID = 0 的 Heatbeat 封包要呼叫 `mav_heartbeat()` 處理 * 可以在官網查詢封包的 ID 編號，例如: [Heartbeat (#0)](https://mavlink.io/en/messages/common.html#HEARTBEAT). ```c= enum ENUM_MAV_CMDS { ENUM_MAVLINK_HANDLER(mav_heartbeat), ENUM_MAVLINK_HANDLER(mav_command_long), ... MAV_CMD_CNT }; struct mavlink_cmd cmd_list[] = { DEF_MAVLINK_CMD(mav_heartbeat, 0), DEF_MAVLINK_CMD(mav_command_long, 76), ... }; void parse_mavlink_msg(mavlink_message_t *msg) { for (int i = 0; i < CMD_LEN(cmd_list); i++) { if (msg->msgid == cmd_list[i].msg_id) { cmd_list[i].handler(msg); return; } } ... } void mav_heartbeat(mavlink_message_t *recvd_msg) { ... } void mav_command_long(mavlink_message_t *recvd_msg) { ... } ``` ### 3-5. 封包的發送: 我們以 `mavlink_send_play_tune()` 說明封包的發送過程: ```c= void mavlink_send_play_tune(void) { uint8_t sys_id = 1; uint8_t component_id = 191; uint8_t target_system = 0; uint8_t target_component = 0; uint8_t format = TUNE_FORMAT_MML_MODERN; const char *tune = "T200 L16 O5 A C O6 E O5 A C O6 E O5 A C O6 E"; mavlink_message_t msg; mavlink_msg_play_tune_v2_pack(sys_id, component_id, &msg, target_system, target_component, format, tune); mavlink_send_msg(&msg); ... } ``` 以下拆成三個步驟說明: 1. 行3至8設定為 `play_tune` 封包所需參數。 2. 行11呼叫 `mavlink_msg_play_tune_v2_pack()` 將參數設定至 `mavlink_message_t msg` 並在進行了諸如設定 Message ID、計算 Checksu 等工作。 3. 透過 `mavlink_send_msg()` 發送至目標裝置。 而 `mavlink_send_msg()` 則是透過 `mavlink_msg_to_send_buffer()` 將 `mavlink_message_t msg` 序列化 (Serialization) 轉為 Byte array 後以 `write()` 送出資料: ```c= void mavlink_send_msg(mavlink_message_t *msg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN]; size_t len = mavlink_msg_to_send_buffer(buf, msg); write(mavlink_fd, buf, len); } ``` 另外 `mavlink_send_play_tune()` 將 `format` 設定為 `TUNE_FORMAT_MML_MODERN (1)`, 因此 tune 的字串必須以 [Music Macro Language](https://zh.wikipedia.org/zh-tw/Music_Macro_Language) 格式表達。 繁中[維基百科的條目](https://zh.wikipedia.org/zh-tw/Music_Macro_Language)給出了詳細易懂的說明: * `CDEFGAB`: 依序表示 Do Re Mi Fa So La Si，後接 `#` 或 `+` 為升記號、`-` 為降記號。 * `R`: 休止符。 * `O`: 指定在哪個八度演奏，影響音調高低。 * `>`、`<`: 控制樂譜高八度(`>`)或低八度(`<`)。 * `L`: 音符時值 (四分音符等)。 * `V`: 指定音量大小，後接的數字指定演奏樂器之音量大小。 * `T`: 指定樂器的速度。例如「T120」表示以120BPM來演奏。 最後，關於封包的詳細定義請見： [PLAY_TUNE_V2 (#400)](https://mavlink.io/en/messages/common.html#PLAY_TUNE_V2)。 ## IV. Microservice 的原理和設計 MAVLink 定義了許多 [Microservice](https://mavlink.io/en/services/) (即需要時序控制的通訊)。本章節錄並以 [Gimbal Protocol (v2)](https://mavlink.io/en/services/gimbal_v2.html) 為例說明開發一個 Microservice 程式的大致流程。 ### 4-1. Gimbal Protocol (v2) 基本概念 基本術語: * Ground Station: 地面站軟體 * Gimbal Manager: 雲台管理/控制軟體，可以是運作在: * 飛控軟體 (Autopilot) 上 * 協同電腦 (Companion Computer) 上，即 Raspberry Pi, Qualcomm RB5 等 * 雲台本體 (Gimbal device) 上 * Gimbal Device: 雲台本體 雲台的連接有三種可能的拓樸結構: **1. 飛控直連雲台 架構:** ![](https://hackmd.io/_uploads/HJSa0Ffhn.png) **2. 獨立式雲台 (Standalone Integrated Camera / Gimbal) 架構:** ![](https://hackmd.io/_uploads/H1TpRYM23.png) **3. 飛控 - 協同電腦 - 雲台裝置 架構:** ![](https://hackmd.io/_uploads/Hym00YM33.png) 其中: * `Gimbal Manager Message` 是給 Gimbal 管理/控制程式之指令封包 * `Gimbal Device Message` 是給 Gimbal 裝置本體之指令封包 以下第三種拓樸結構 (飛控 - 協同電腦 - 雲台裝置) 作為說明。 ### 4-3. 封包流程 (Sequence) 如上所述，考慮第三種拓樸，假設要設計協同電腦上的 MAVLink 程式: * Gimbal Manager 運作在協同電腦上，對 Ground Station 自然是透過 MAVLink 的 `Gimbal Manager Message` 協議交換資訊。 * Gimbal Device 考慮以下兩種情形: * 支援 MAVLink: Manager 對 Device 使用 `Gimbal Device Message` 通訊 * 不支援 MAVLink: Manager 必須透過其他方式 (TCP/IP, RTPS, Serial 等) 通訊 以下為 Gimbal Protocol (v2) 定義的四種流程 (Sequence): **1. 裝置探索 (Discovery):** ![](https://hackmd.io/_uploads/H1pHz9fnh.png) * [COMMAND_LONG (#76)](https://mavlink.io/en/messages/common.html#COMMAND_LONG) --- 控制指令，具體是哪一個行為要設定 * [MAV_CMD_REQUEST_MESSAGE (512)](https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE) --- 代入 COMMAND_LONG 中使用 * [GIMBAL_MANAGER_INFORMATION (#280)](https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION) * [GIMBAL_DEVICE_INFORMATION (#283)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION) * [COMMAND_ACK (#77)](https://mavlink.io/en/messages/common.html#COMMAND_ACK) **2. 一般手動操作 (Normal Manual Control):** 指的是透過操作者透過地面站或是遙控器 (RC, Remote Controller) 控制雲台。 ![](https://hackmd.io/_uploads/r1zUzcz3h.png) * [GIMBAL_MANAGER_SET_ATTITUDE (#282)](https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE) * [GIMBAL_DEVICE_SET_ATTITUDE (#284)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE) * [GIMBAL_DEVICE_ATTITUDE_STATUS (#285)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS) **3. 透過地面站設定感興趣區間 (ROI, Region of Interest)** ![](https://hackmd.io/_uploads/BJ5379G2h.png) * [COMMAND_LONG (#76)](https://mavlink.io/en/messages/common.html#COMMAND_LONG) --- 控制指令，具體是哪一個行為要設定 * [MAV_CMD_DO_SET_ROI_LOCATION (195)](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION) --- 代入 COMMAND_LONG 中使用 * [GIMBAL_DEVICE_SET_ATTITUDE (#284)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE) * [GIMBAL_DEVICE_ATTITUDE_STATUS (#285)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS) * [MAV_CMD_DO_SET_ROI_NONE (197)](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE) --- 代入 COMMAND_LONG 中使用 * [COMMAND_ACK (#77)](https://mavlink.io/en/messages/common.html#COMMAND_ACK) **4. 透過自動飛行任務排程的姿態設定 (Attitude Set During Mission):** **Caveat:** 官方給的圖應該有誤: 沒有 `CMD_DO_GIMBAL_MANAGER_ATTITUDE`，只有 `CMD_DO_GIMBAL_MANAGER_PITCHYAW`。 不過根據圖的繪製來說，此封包的發送應可不用實作出來。 ![](https://hackmd.io/_uploads/Sk6If5z2h.png) * [COMMAND_LONG (#76)](https://mavlink.io/en/messages/common.html#COMMAND_LONG) --- 控制指令，具體是哪一個行為要設定 * [MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW (1000)](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) --- 代入 COMMAND_LONG 中使用 * [GIMBAL_DEVICE_SET_ATTITUDE (#284)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE) * [GIMBAL_DEVICE_ATTITUDE_STATUS (#285)](https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS) ### 4-3. COMMAND_LONG 的接收處理 MAVLink 的 Command 有分 [COMMAND_LONG (#76)](https://mavlink.io/en/messages/common.html#COMMAND_LONG) 和 [COMMAND_INT (#75)](https://mavlink.io/en/messages/common.html#COMMAND_INT)。前者的7個參數欄位都是 `float` 型態，後者的第1, 2, 3, 4, 7欄是float，5和6是 `int32_t` 型態。 [COMMAND_INT (#75)](https://mavlink.io/en/messages/common.html#COMMAND_INT) 主要用於 GPS 相關的指令，因為在儲存經緯度上，`int32_t` 較 `float` 有更高的精度優勢。除此之外絕大多數的功能都是使用 [COMMAND_LONG (#76)](https://mavlink.io/en/messages/common.html#COMMAND_LONG)。 COMMAND_LONG 的接收處理程式如下: ```c= void mav_command_long(mavlink_message_t *recvd_msg) { ... /* Decode command_long message */ mavlink_command_long_t mav_cmd_long; mavlink_msg_command_long_decode(recvd_msg, &mav_cmd_long); /* Ignore the message if the target id does not match the system id */ if (get_sys_id() != mav_cmd_long.target_system) { return; } switch (mav_cmd_long.command) { case MAV_CMD_DO_SET_ROI_LOCATION: /* 195 */ // mav_cmd_do_set_roi_location_handler(&mav_cmd_long); break; case MAV_CMD_DO_SET_ROI_NONE: /* 197 */ // mav_cmd_do_set_roi_none_handler(&mav_cmd_long); break; case MAV_CMD_REQUEST_MESSAGE: /* 512 */ // mav_cmd_request_msg_handler(&mav_cmd_long); break; } } ``` 實際上就是經 3-4 章介紹之 `parse_mavlink_msg()` 函式觸發 `mav_command_long()` 後再根據`mav_cmd_long.command` 的數值選擇對應的處理函式。 ### 4-4. Microservice 的程式設計 我們以上述 **透過地面站設定感興趣區間 (ROI, Region of Interest)** 的協議進行說明 ![](https://hackmd.io/_uploads/BJ5379G2h.png) 首先根據圖示可知 `mavlink_parser.c` 中必須先增加以下幾個封包的處理函式: * `COMMAND_LONG (#76)` → `MAV_CMD_DO_SET_ROI_LOCATION (195)` * `GIMBAL_DEVICE_ATTITUDE_STATUS (#285)` * `COMMAND_LONG (#76)` → `MAV_CMD_DO_SET_ROI_NONE (197)` 接著便可採 Event-driven 架構撰寫程式並做出適當的回應 (如發送封包回應)。 但實做上可能還須考量: 1. 是否加上狀態機追蹤 2. 目標沒有正確收到 `COMMAND_ACK (#77)` 時的處理 **情況1** 須根據開發者自行斟酌，以下說明 **情況2**: 由於 `COMMAND_ACK (#77)` 的回傳值包含 `ACCEPTED`、`DENIED` 等 (見[MAV_RESULT](https://mavlink.io/en/messages/common.html#MAV_RESULT))，而目標程式可能因資料遺失等重新要求，因此會需要保存特定資料。 顯然的，在此處 Gimbal 的案例中 `COMMAND_ACK (#77)` 的結果是依據`GIMBAL_DEVICE_ATTITUDE_STATUS (#285)` 而定的: ![](https://hackmd.io/_uploads/Sk4Iz3M23.png) 即使 `MAV_CMD_DO_SET_ROI_NONE (197)` 被多次觸發，Gimbal Manager 也應該有辦法正確回應 `COMMAND_ACK (#77)` 封包。
Last changed by  
 
Shengwen Cheng
725

Read more

My OnlineCourseWare Collections

MIT: 8.01 Physics I: Classical Mechanics

5/14/2024
Embedded Open Source Summit 2024 會後紀錄

四月十六日在黃敬群老師指導下，我們在美國西雅圖由 Linux Foundation 主辦的 Embedded Open Source Summit (EOSS) 中發表了以下針對無人機系統整合的題目，歡迎有興趣的朋友參考。連結:

5/8/2024
Curriculum Vitae

[中文版]

3/7/2024
在 STM32 上使用 NuttX 的實驗筆記

qemu-system-arm -M netduinoplus2 -kernel nuttx.bin -serial pty -serial stdio

9/13/2023
Read more from Shengwen Cheng
Published on HackMD