前言

APM中MAVLINK的傳輸和控制主要實現在libraries/GCS_MAVLINK以及對應的車輛資料夾下（如Ardusub/）。總的來說，一般在libraries下面，各個資料夾表示不同的庫模組，其中各自庫中與庫重名的檔案往往是作為庫內檔案與車輛上層的前端而存在，而在車輛資料夾下面，對應的GCS.h/.cpp和GCS_Mavlink.h/.cpp檔案是作為上層，它們繼承了庫中實現的類和相應的功能，並以此為基礎派生出專屬於具體車輛型別的函數及功能（簡單來說就是繼承了庫中的基礎類別由此得到的派生類）。

一、_chan

在開始之前，需要先了解MAVLINK的傳輸控制是如何實現的。

定位到/libraries/GCS_MAVLink/GCS.h檔案，內部實現了GCS_MAVLINK類和GCS類，需要注意class GCS_MAVLINK是MAVLink傳輸控制類，而class GCS相當於在pixhawk內部實現了一個類似地面站的功能（你可以這樣理解：我們電腦作為地面站需要接受pixhawk傳送給我們的mavlink訊息，而pixhawk在接收mavlink訊息的時候，內部程式也需要實現一個地面站的功能用來實現接收）。

兩個類中的內容很多，這裡就不全部放上來了。需要關注的重點是在GCS類中

    GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];
    uint8_t _num_gcs;
    GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];

先來看最後一行定義的_chan，這是一個GCS_MAVLINK類生成的指標陣列物件，其中在GCS_MAVLink.h中有定義如下，意思就是說對於mavlink我們最多隻能有5個介面。

// allow five telemetry ports
#define MAVLINK_COMM_NUM_BUFFERS 5

然後回到開頭，該部分定義了針對每一個介面的介面引數，GCS_MAVLINK_Parameters同樣實現在GCS.h中

#define GCS_MAVLINK_NUM_STREAM_RATES 10
class GCS_MAVLINK_Parameters
{
public:

    GCS_MAVLINK_Parameters();

    static const struct AP_Param::GroupInfo        var_info[];

    // saveable rate of each stream
    AP_Int16        streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
};

二、init_ardupilot()

讓我們重新回到init_ardupilot()，這部分內容雖然在Ardusub原始碼解析學習（四）——IMU中有所表述，但我們只是說了IMU的部分，這次我們重頭再來說一下MAVLINK的初始化部分（這邊就不把init_ardupilot()的全部程式給貼出來了）。

2.1 SYSID

關注程式開始部分的這段

    // identify ourselves correctly with the ground station
    mavlink_system.sysid = g.sysid_this_mav;

這段程式碼就是設定mavlink的系統編號來保證地面站能夠正確識別自己。mavlink2.0版本的格式如下：
在這裡插入圖片描述
那麼該段程式的作用就是設定其中的SYSID塊。

/**
 * This structure is required to make the mavlink_send_xxx convenience functions
 * work, as it tells the library what the current system and component ID are.
 */
MAVPACKED(
typedef struct __mavlink_system {
    uint8_t sysid;   ///< Used by the MAVLink message_xx_send() convenience function
    uint8_t compid;  ///< Used by the MAVLink message_xx_send() convenience function
}) mavlink_system_t;

/

mavlink_system_t mavlink_system = {7,1};

也就是說mavlink_system這個在初始化宣告的時候，預設sysid=7,compid=1。但是此時經過上面的賦值語句之後：

g是在Sub類中宣告的一個Parameters物件，在Parameters.cpp中實現了sysid_this_mav的預設賦值

    // @Param: SYSID_THISMAV
    // @DisplayName: MAVLink system ID of this vehicle
    // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
    // @Range: 1 255
    // @User: Advanced
    GSCALAR(sysid_this_mav, "SYSID_THISMAV",   MAV_SYSTEM_ID),

其中MAV_SYSTEM_ID=1。那麼最上面的語句就是將mavlink_system.sysid這個值賦為1。在mavlink訊息中，sysid=1預設指代韌體刷寫的pixhawk系統。

2.2 setup_console()

    // identify ourselves correctly with the ground station
    mavlink_system.sysid = g.sysid_this_mav;
    
    // initialise serial port
    serial_manager.init();

    // setup first port early to allow BoardConfig to report errors
    gcs().setup_console();

回到ardupilot_init()下，serial_manager.init()實現了對串列埠的初始化。但我們本次主要討論的是mavlink相關的，所以接下來看一下gcs().setup_console()的實現。這個函數的具體作用根據備註的意思就是：儘早設定第一個埠，以允許BoardConfig報告錯誤。

在Sub.h下的Sub類中，有宣告物件如下。其中GCS_Sub是GCS類在Ardusub中的具體派生類。

    // GCS selection
    GCS_Sub _gcs; // avoid using this; use gcs()
    GCS_Sub &gcs() { return _gcs; }

setup_console()是GCS類中的一個函數，實現在GCS_Common.cpp中

void GCS::setup_console()
{
    AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, 0);
    if (uart == nullptr) {
        // this is probably not going to end well.
        return;
    }
    if (ARRAY_SIZE(chan_parameters) == 0) {
        return;
    }
    create_gcs_mavlink_backend(chan_parameters[0], *uart);
}

find_serial()功能概述：在可用的串列埠搜尋允許給定協定的第一個範例（instance），如果搜尋的是協定的第一個範例，那麼範例（instance）應該為0，第二個為1，以此類推。成功則返回串列埠裝置

// find_serial - searches available serial ports for the first instance that allows the given protocol
//  instance should be zero if searching for the first instance, 1 for the second, etc
//  returns uart on success, nullptr if a serial port cannot be found
AP_HAL::UARTDriver *AP_SerialManager::find_serial(enum SerialProtocol protocol, uint8_t instance) const
{
    const struct UARTState *_state = find_protocol_instance(protocol, instance);
    if (_state == nullptr) {
        return nullptr;
    }
    return _state->uart;
}

回到setup_console()中，接下來就是對是否檢測到串列埠裝置及chan引數進行判斷，沒有就直接返回，有的話那麼就呼叫 create_gcs_mavlink_backend()函數。這個函數的作用就是根據給定的chan_parameters[]中的引數以及獲取到的串列埠生成新的GCS_MAVLINK_XXX（XXX表示具體車輛型別，繼承自GCS_MAVLINK）物件儲存在_chan[]中，該部分作用通過 new_gcs_mavlink_backend()這個函數完成。然後通過init()函數實現介面類的初始化工作，如果初始化失敗，就刪除這個介面。

void GCS::create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params, AP_HAL::UARTDriver &uart)
{
    if (_num_gcs >= ARRAY_SIZE(chan_parameters)) {
        return;
    }
    _chan[_num_gcs] = new_gcs_mavlink_backend(params, uart);
    if (_chan[_num_gcs] == nullptr) {
        return;
    }

    if (!_chan[_num_gcs]->init(_num_gcs)) {
        delete _chan[_num_gcs];
        _chan[_num_gcs] = nullptr;
        return;
    }

    _num_gcs++;
}

2.3 setup_uarts()

回到init_ardupilot()中，忽略掉後面一些函數，我們直接來看這兩個

    // Register the mavlink service callback. This will run
    // anytime there are more than 5ms remaining in a call to
    // hal.scheduler->delay.
    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);

    // setup telem slots with serial ports
    gcs().setup_uarts();

先看 gcs().setup_uarts()，與前面解釋的setup_console()函數類似，這邊實現的是對剩下4個類介面的註冊及初始化工作。後面這部分筆者也尚未看懂，後面有機會在更新，有懂的大佬麻煩講解一些。但是重點只要知道這個函數完成了APM韌體中「地面站」介面的註冊及初始化即可。

void GCS::setup_uarts()
{
    for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
        if (i >= ARRAY_SIZE(chan_parameters)) {
            // should not happen
            break;
        }
        AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, i);
        if (uart == nullptr) {
            // no more mavlink uarts
            break;
        }
        create_gcs_mavlink_backend(chan_parameters[i], *uart);
    }

    if (frsky == nullptr) {
        frsky = new AP_Frsky_Telem();
        if (frsky == nullptr || !frsky->init()) {
            delete frsky;
            frsky = nullptr;
        }
    }

#if !HAL_MINIMIZE_FEATURES
    devo_telemetry.init();
#endif
}

2.4 mavlink_delay_cb()

回到原函數中，hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5)註冊mavlink服務回撥。這將在對hal.scheduler-> delay的呼叫中剩餘超過5毫秒的任何時間執行。這個函數將呼叫sub.mavlink_delay_cb();

對於這個函數，官方的解釋是處理MAVLink封包的delay（）回撥。我們在長時間執行的庫初始化例程中將其設定為回撥，以允許MAVLink在等待初始化完成的同時處理封包。函數實現如下：

/*
 *  a delay() callback that processes MAVLink packets. We set this as the
 *  callback in long running library initialisation routines to allow
 *  MAVLink to process packets while waiting for the initialisation to
 *  complete
 */
void Sub::mavlink_delay_cb()
{
    static uint32_t last_1hz, last_50hz, last_5s;

    logger.EnableWrites(false);

    uint32_t tnow = AP_HAL::millis();
    if (tnow - last_1hz > 1000) {
        last_1hz = tnow;
        gcs().send_message(MSG_HEARTBEAT);
        gcs().send_message(MSG_SYS_STATUS);
    }
    if (tnow - last_50hz > 20) {
        last_50hz = tnow;
        gcs().update_receive();
        gcs().update_send();
        notify.update();
    }
    if (tnow - last_5s > 5000) {
        last_5s = tnow;
        gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
    }

    logger.EnableWrites(true);
}

注意到每次以ms為單位獲取tnow時間，並且以1Hz頻率（1s時間）通過APM韌體中的「地面站」向實際電腦地面站傳送心跳包和系統狀態包，MSG_HEARTBEAT和MSG_SYS_STATUS表示id號，分別為1和3。按50hz頻率進行傳送和接收的更新。每5s傳送一次資訊。這裡面呼叫的函數內容有點多，後面找機會另寫一篇研究一下，這次就先到這吧。