Try   HackMD

MAVLink 原理及開發指南

I. 範例程式及使用說明

GitHub




git clone https://github.com/shengwen-tw/mavlink_example
cd mavlink_example/
make all

1-1. 使用 USB-TTL 模組進行 Loopback 測試:

將 TXD, RXD 短路:

Image Not Showing Possible Reasons
  • The image was uploaded to a note which you don't have access to
  • The note which the image was originally uploaded to has been deleted
Learn More →

操作:

./mavlink -v -s /dev/ttyUSBX -b 115200

1-2.作為 TCP/IP Client 連線:

./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 頁面找到。

以下給出 Codegen 產生程式碼的操作流程:








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 程式本體。另外這裡也提供預先產生好的結果。

註: 下述的範例程式已包含了 MAVLink 程式碼，不需要再額外產生。

III. 基本原理

在 MAVLink 中無論是發送或是接收，都僅是操作以下的資料結構:

Image Not Showing Possible Reasons
  • The image was uploaded to a note which you don't have access to
  • The note which the image was originally uploaded to has been deleted
Learn More →

以下幾個部分值得注意:

  • SYS ID: System ID，用於標註發送者，0 表示 Broadcasting, 一般使用 1-255。
  • COMP ID: Component ID, 標示發送者具體是什麼元件，例如可以設定為 Autopilot 加上 Camera (見 MAV_COMPONENT)
  • MSG ID: 封包的編號，例如 0 是代表 Heatbeat
  • Payload: 數據區塊

Remark: 透過檢查 SYS ID 和 COMP ID 可以確定封包的來源。

詳細說明可見: Packet Serialization

範例程式使用了兩個執行緒處理 MAVLink 封包 (Message) 的收發。

  • 不斷嘗試讀取新的 Byte 並傳入 mavlink_parse_char() 函式進行解碼
  • 將解碼成功後的 recvd_msg 透過 Message queue 傳至 mavlink_tx_thread() 中做出回應
















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));
        }
    }
}
  • 固定時間發送特定封包 (由 MSG_SCHEDULER_INIT() 和 MSG_SEND_HZ() 巨集函式控制)
  • 從 Message queue 取出在 mavlink_rx_thread 接收到 (解出) 的封包並進行回應。
    • 回應的程式最簡單的只要回應 Ack 封包，較複雜的 (即 Microservice) 則要根據功能維護一個狀態機並進行合適的回覆。
    • parse_mavlink_msg() 是用於根據接收到的 Message ID 觸發對應的 MAVLink Handler，若是跟 Microservice 有關則必須再改變對應的狀態機狀態。
    • 狀態機改變後，Microservice handler 必須再做出回應。
  • mavlink_tx_thread() 每 10ms 僅能觸發一次，用以限制 CPU 資源使用 (經驗上來說 100Hz 以足夠充分使用)
























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
    }
}
  • 在 mavlink_parser.c 中新增 MAVLink handler function 用於回應接收到的封包
    • 例: ID = 0 的 Heatbeat 封包要呼叫 mav_heartbeat() 處理
    • 可以在官網查詢封包的 ID 編號，例如: Heartbeat (#0).

































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() 說明封包的發送過程:
















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() 送出資料:








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 格式表達。

繁中維基百科的條目給出了詳細易懂的說明:

  • CDEFGAB: 依序表示 Do Re Mi Fa So La Si，後接 # 或 + 為升記號、- 為降記號。
  • R: 休止符。
  • O: 指定在哪個八度演奏，影響音調高低。
  • >、<: 控制樂譜高八度(>)或低八度(<)。
  • L: 音符時值 (四分音符等)。
  • V: 指定音量大小，後接的數字指定演奏樂器之音量大小。
  • T: 指定樂器的速度。例如「T120」表示以120BPM來演奏。

最後，關於封包的詳細定義請見： PLAY_TUNE_V2 (#400)。

IV. Microservice 的原理和設計

MAVLink 定義了許多 Microservice (即需要時序控制的通訊)。本章節錄並以 Gimbal Protocol (v2) 為例說明開發一個 Microservice 程式的大致流程。

4-1. Gimbal Protocol (v2) 基本概念

基本術語:

  • Ground Station: 地面站軟體
  • Gimbal Manager: 雲台管理/控制軟體，可以是運作在:
    • 飛控軟體 (Autopilot) 上
    • 協同電腦 (Companion Computer) 上，即 Raspberry Pi, Qualcomm RB5 等
    • 雲台本體 (Gimbal device) 上
  • Gimbal Device: 雲台本體

雲台的連接有三種可能的拓樸結構:

1. 飛控直連雲台 架構:

2. 獨立式雲台 (Standalone Integrated Camera / Gimbal) 架構:

3. 飛控 - 協同電腦 - 雲台裝置 架構:

其中:

  • 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):

2. 一般手動操作 (Normal Manual Control):

指的是透過操作者透過地面站或是遙控器 (RC, Remote Controller) 控制雲台。

3. 透過地面站設定感興趣區間 (ROI, Region of Interest)

4. 透過自動飛行任務排程的姿態設定 (Attitude Set During Mission):

Caveat: 官方給的圖應該有誤: 沒有 CMD_DO_GIMBAL_MANAGER_ATTITUDE，只有 CMD_DO_GIMBAL_MANAGER_PITCHYAW。

不過根據圖的繪製來說，此封包的發送應可不用實作出來。

4-3. COMMAND_LONG 的接收處理

MAVLink 的 Command 有分 COMMAND_LONG (#76) 和 COMMAND_INT (#75)。前者的7個參數欄位都是 float 型態，後者的第1, 2, 3, 4, 7欄是float，5和6是 int32_t 型態。

COMMAND_INT (#75) 主要用於 GPS 相關的指令，因為在儲存經緯度上，int32_t 較 float 有更高的精度優勢。除此之外絕大多數的功能都是使用 COMMAND_LONG (#76)。

COMMAND_LONG 的接收處理程式如下:

























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) 的協議進行說明

首先根據圖示可知 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)，而目標程式可能因資料遺失等重新要求，因此會需要保存特定資料。

顯然的，在此處 Gimbal 的案例中 COMMAND_ACK (#77) 的結果是依據GIMBAL_DEVICE_ATTITUDE_STATUS (#285) 而定的:

即使 MAV_CMD_DO_SET_ROI_NONE (197) 被多次觸發，Gimbal Manager 也應該有辦法正確回應 COMMAND_ACK (#77) 封包。

Last changed by 
 
Shengwen Cheng
0
3
3503

Read more

My OnlineCourseWare Collections

MIT: 8.01 Physics I: Classical Mechanics

Jun 3, 2025
TOEFL Preparation

contingency

Dec 4, 2024
Embedded Open Source Summit 2024 會後紀錄

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

May 8, 2024
在 STM32 上使用 NuttX 的實驗筆記

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

Sep 13, 2023
Read more from Shengwen Cheng
Published on HackMD