MAVLINK在Ardupilot中的初始化过程

前言
一、_chan
二、init_ardupilot()

本文将以ArduSub为例写一下在APM中MAVLINK是如何实现初始化的，其他车辆类型实现过程是基本一致的，也可当作参考。如有错误请及时告知。

前言

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发送一次信息。这里面调用的函数内容有点多，后面找机会另写一篇研究一下，这次就先到这吧。

MAVLINK在Ardupilot中的初始化过程

MAVLINK在Ardupilot中的初始化过程

前言

一、_chan

二、init_ardupilot()

2.1 SYSID

2.2 setup_console()

2.3 setup_uarts()

2.4 mavlink_delay_cb()

猜你喜欢