ArduPilot 第3章 MAVLINK协议

已于 2024-05-21 16:20:24 修改
阅读量2.2k 收藏 64
点赞数 51
文章标签： 算法
于 2024-02-19 11:08:41 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zhanggeng0101/article/details/136162162
版权

文章目录

参考文献

https://blog.csdn.net/moumde/article/details/109061049
MAVLINK在Ardupilot中的初始化过程

ardupilot — 从mavlink消息到底层参数赋值篇
https://blog.csdn.net/weixin_43321489/article/details/132236353

Ardusub源码解析学习(三)——车辆类型-CSDN博客
http://www.360doc.com/content/23/1003/04/1676247_1098781980.shtml

MAVLINK消息在Ardupilot中的接收和发送过程
https://blog.csdn.net/moumde/article/details/109209297

QGC添加自定义组件和发送自定义MAVLINK消息
https://blog.csdn.net/moumde/article/details/109112051#MAVLINK_157

Ardupilot自定义mavlink消息
https://blog.csdn.net/PENG__WANG/article/details/123696033

如何定义 MAVLink 消息（Messages）& 枚举（Enums）
https://mavlink.io/zh/guide/define_xml_element.html

前言

本文主要介绍ArduPilot 中的MAVLINK协议。

一、实现位置

APM中MAVLINK的传输和控制主要实现在libraries/GCS_MAVLINK以及对应的车辆文件夹下（如Ardusub/）。
(1)在libraries下面，各个文件夹表示不同的库模块，其中各自库中与库重名的文件往往是作为库内文件与车辆上层的前端而存在，
(2)在车辆文件夹下面，对应的GCS.h/.cpp和GCS_Mavlink.h/.cpp文件是作为上层，它们继承了库中实现的类和相应的功能，并以此为基础派生出专属于具体车辆类型的函数及功能（简单来说就是继承了库中的基类由此得到的派生类）。

二、传输控制

1.定义接口参数

(1)定位到/libraries/GCS_MAVLink/GCS.h文件，内部实现了GCS_MAVLINK类和GCS类。

(2)class GCS_MAVLINK是MAVLink传输控制类，而class GCS相当于在pixhawk内部实现了一个类似地面站的功能（电脑作为地面站需要接受pixhawk发送给我们的mavlink消息，而pixhawk在接收mavlink消息的时候，内部程序也需要实现一个地面站的功能用来实现接收）。

(3)在GCS类中，
在\libraries\GCS_MAVLink\GCS.h
请添加图片描述
请添加图片描述
先来看最后一行定义的_chan，这是一个GCS_MAVLINK类生成的指针数组对象，其中在GCS_MAVLink.h中有定义如下，意思就是说对于mavlink我们最多只能有5个接口。
\libraries\GCS_MAVLink\GCS_MAVLink.h

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// allow extra mavlink channels in SITL for:
//    Vicon, ship
#define MAVLINK_COMM_NUM_BUFFERS 7
#else
// allow five telemetry ports
#define MAVLINK_COMM_NUM_BUFFERS 5
#endif

(4)然后回到开头，该部分定义了针对每一个接口的接口参数，GCS_MAVLINK_Parameters同样实现在GCS.h中
在\libraries\GCS_MAVLink\GCS.h
请添加图片描述

2.MAVLINK初始化

在libraries\AP_Vehicle\AP_Vehicle.cpp中
AP_Vehicle.cpp对setup()进行具体实现的时候调用了init_ardupilot()。
Sub类继承自AP_Vehicle类，并且并未对setup()和loop()进行修改或重写。相对应的，其在内部以private方式继承了fast_loop()和init_ardupilot()。

已知Sub类继承了AP_Vehicle类所实现的函数，那么Sub::setup()函数在运行过程中会调用void AP_Vehicle::setup()中的gcs().init()

(1)

在\libraries\AP_Vehicle\AP_Vehicle中
在void AP_Vehicle::setup()函数中调用gcs().init()
gcs().init()

void AP_Vehicle::setup()
{
	...
	// initialise serial manager as early as sensible to get
    // diagnostic output during boot process.  We have to initialise
    // the GCS singleton first as it sets the global mavlink system ID
    // which may get used very early on.
    gcs().init();
    ...
}

(2)
在\libraries\GCS_MAVLink\GCS_Common.cpp中
GCS &gcs()

GCS &gcs()
{
    return *GCS::get_singleton();
}

(3)
在\libraries\GCS_MAVLink\GPS.cpp中
这段代码就是设置mavlink的系统编号来保证地面站能够正确识别自己。mavlink2.0版本的格式如下
void GCS::init()

void GCS::init()
{
    mavlink_system.sysid = sysid_this_mav();
}

请添加图片描述
那么该段程序的作用就是设置其中的SYSID块。

(4)
在\build\CubeOrange\libraries\GCS_MAVLink\include\mavlink\v2.0\mavlink_types.h
这段代码就是设置mavlink的系统编号来保证地面站能够正确识别自己。mavlink2.0版本的格式如下
mavlink_system_

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

(5)
在\libraries\GCS_MAVLink\GCS_MAVLink.cpp
mavlink_system这个在初始化声明的时候，默认sysid=7,compid=1。但是此时经过上面的赋值语句之后：
g是在Sub类中声明的一个Parameters对象，在Parameters.cpp中实现了sysid_this_mav的赋值

mavlink_system_t mavlink_system = {7,1};

(6)
在ArduCopter\GCS_Copter.cpp中

uint8_t GCS_Copter::sysid_this_mav() const

uint8_t GCS_Copter::sysid_this_mav() const
{
    return copter.g.sysid_this_mav;
}

(7)
在\ArduCopter\Parameters.cpp中

g是在Sub类中声明的一个Parameters对象，在Parameters.cpp中实现了sysid_this_mav的赋值。
其中MAV_SYSTEM_ID=1。那么最上面的语句就是将mavlink_system.sysid这个值赋为1。
const AP_Param::Info Copter::var_info[]

const AP_Param::Info Copter::var_info[] = {
    ...
    // @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),
    ...

(8)
在\libraries\AP_Vehicle\AP_Vehicle.cpp中

serial_manager.init()实现了对串口的初始化。本次主要讨论的是mavlink相关的，所以接下来看一下gcs().setup_console()的实现。这个函数的具体作用：尽早设置第一个端口，以允许BoardConfig报告错误。
gcs().setup_console()

void AP_Vehicle::setup()
{
    ...
  	gcs().init();

    // initialise serial ports
    serial_manager.init();
    gcs().setup_console();
    ...

(9)
在ArduCopter\Copter.h中
在Copter.h下的Copter类中，有声明对象如下。其中GCS_Copter是GCS类在ArduCopter中的具体派生类。

GCS_Copter _gcs;
GCS_Copter &gcs() { return _gcs; }

 class Copter : public AP_Vehicle {
 ...
 private:
 	...
 	// GCS selection
	GCS_Copter _gcs; // avoid using this; use gcs()
    GCS_Copter &gcs() { return _gcs; }
...
}

(10)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

setup_console()是GCS类中的一个函数，实现在GCS_Common.cpp中
gcs().setup_console()

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);
}

(11)
在\libraries\AP_SerialManager\AP_SerialManager.cpp中

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;
    }
    const uint8_t serial_idx = _state - &state[0];

    // set options before any user does begin()
    AP_HAL::UARTDriver *port = hal.serial(serial_idx);
    if (port) {
        port->set_options(_state->options);
    }
    return port;
}

(12)
在\libraries\AP_SerialManager\AP_SerialManager.cpp中
find_protocol_instance(enum SerialProtocol protocol, uint8_t instance)

const AP_SerialManager::UARTState *AP_SerialManager::find_protocol_instance(enum SerialProtocol protocol, uint8_t instance) const
{
    uint8_t found_instance = 0;

    // search for matching protocol
    for(uint8_t i=0; i<SERIALMANAGER_NUM_PORTS; i++) {
        if (protocol_match(protocol, (enum SerialProtocol)state[i].protocol.get())) {
            if (found_instance == instance) {
                return &state[i];
            }
            found_instance++;
        }
    }

    // if we got this far we did not find the uart
    return nullptr;
}

(13)
在\libraries\GCS_MAVLink\GCS_Common.cpp中
回到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++;
}

(14)
在\libraries\AP_Vehicle\AP_Vehicle.cpp中
hal.scheduler->register_delay_callback(scheduler_delay_callback, 5)
注册mavlink服务回调。 这将在对hal.scheduler-> delay的调用中剩余超过5毫秒的任何时间运行。这个函数将调用scheduler_delay_callback

对于这个函数，官方的解释是处理MAVLink数据包的delay（）回调。 我们在长时间运行的库初始化例程中将其设置为回调，以允许MAVLink在等待初始化完成的同时处理数据包。函数实现如下：

void AP_Vehicle::setup()
{
	...
	// Register scheduler_delay_cb, which will run anytime you have
    // more than 5ms remaining in your call to hal.scheduler->delay
    hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
	...
}

(15)
在\libraries\AP_Vehicle\AP_Vehicle.cpp中

void AP_Vehicle::scheduler_delay_callback()
对于这个函数，官方的解释是处理MAVLink数据包的delay（）回调。 我们在长时间运行的库初始化例程中将其设置为回调，以允许MAVLink在等待初始化完成的同时处理数据包。函数实现如下：

void AP_Vehicle::scheduler_delay_callback()
{
#if APM_BUILD_TYPE(APM_BUILD_Replay)
    // compass.init() delays, so we end up here.
    return;
#endif

    static uint32_t last_1hz, last_50hz, last_5s;

    AP_Logger &logger = AP::logger();

    // don't allow potentially expensive logging calls:
    logger.EnableWrites(false);

    const 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();
        _singleton->notify.update();
    }
    if (tnow - last_5s > 5000) {
        last_5s = tnow;
        if (AP_BoardConfig::in_config_error()) {
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Config Error: fix problem then reboot");
        } else {
            gcs().send_text(MAV_SEVERITY_INFO, "Initialising Ar

(16)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void 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
    ltm_telemetry.init();
#endif

#if AP_DEVO_TELEM_ENABLED
    devo_telemetry.init();
#endif
}

三、接收发送

1.接收

(1)
在\ArduCopter\Copter.cpp中
ArduCopter路径下，Copter.cpp中，对应文件下始终实现了两个函数的任务调度器。这两个函数始终以400Hz的频率运行，在APM上进行MAVLINK消息的发送和接收。

SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
	...
	SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550, 105),
    ...

}

(2)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS::update_receive(void)

这个函数实现在ardupilot/libraries/GCS_MAVLINK/GCS_Common.cpp文件中。GCS类相当于是在APM中实现了一个“地面站”的功能，用于发送和接收消息（当然并不完全正确）。具体来看里面的内容：
num_gcs()获取到总共的MAVLINK数据传输接口（或链路）。然后对于每一个通道执行更新接收操作update_receive()。这个函数才是实际上对于每一个通道接口真正执行接收MAVLINK消息的函数。

void GCS::update_receive(void)
{
    for (uint8_t i=0; i<num_gcs(); i++) {
        chan(i)->update_receive();
    }
    // also update UART pass-thru, if enabled
    update_passthru();
}

(3)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS_MAVLINK::update_receive(uint32_t max_time_us)

这部分代码有点长就不全部放上来了，实际上对于MAVLINK接收消息并且解包的过程，挑重点说一下，首先是在原函数中找到以下该段程序。
其他的可以先不追究细节，重点找到里面的
packetReceived(status, msg);

void
GCS_MAVLINK::update_receive(uint32_t max_time_us)
{
    ...
    // Try to get a new message
        if (mavlink_parse_char(chan, c, &msg, &status)) {
            hal.util->persistent_data.last_mavlink_msgid = msg.msgid;
            packetReceived(status, msg);
            parsed_packet = true;
            gcs_alternative_active[chan] = false;
            alternative.last_mavlink_ms = now_ms;
            hal.util->persistent_data.last_mavlink_msgid = 0;
        }
     ...
}

(4)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
const mavlink_message_t &msg)

packetReceived() 实现了对于MAVLINK消息包的接收。前面一些内容大家看注释理解一下即可。我们只需要关注，packetReceived()函数在最后面调用了 handleMessage()这个函数。这个函数才是对于接收到的消息包进行真正处理的函数。

void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
                                 const mavlink_message_t &msg)
{
    // we exclude radio packets because we historically used this to
    // make it possible to use the CLI over the radio
    if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
        mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
    }
    if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
        (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
        AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {
        // if we receive any MAVLink2 packets on a connection
        // currently sending MAVLink1 then switch to sending
        // MAVLink2
        mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
        if (cstatus != nullptr) {
            cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
        }
    }
    if (!routing.check_and_forward(chan, msg)) {
    // the routing code has indicated we should not handle this packet locally
        return;
    }
    if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
        handle_mount_message(msg);
    }
    if (!accept_packet(status, msg)) {
        // e.g. enforce-sysid says we shouldn't look at this packet
        return;
    }
    handleMessage(msg);
}

定位到该函数定义于库内的GCS.h文件中
在GCS_MAVLINK类中实现为一个虚函数，因此我们必须在各个具体的车辆类型中找到对应派生类中的具体实现。

class GCS_MAVLINK
{
...
private:
	virtual void        handleMessage(const mavlink_message_t &msg) = 0;	
...
}

(5)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)

以ArduCopter为例，在上层车辆文件夹中的GCS_Mavlink.h中实现了GCS_MAVLINK_Copter类对于GCS_MAVLINK消息的继承，并且在GCS_Mavlink.cpp文件中实现了对于handleMessage()的重写。
可以看到在内部通过switch-case语句实现了根据具体的mavlink消息id进行不同的事件处理的功能。

void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
{
    // for mavlink SET_POSITION_TARGET messages
    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
        POSITION_TARGET_TYPEMASK_X_IGNORE |
        POSITION_TARGET_TYPEMASK_Y_IGNORE |
        POSITION_TARGET_TYPEMASK_Z_IGNORE;

    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE =
        POSITION_TARGET_TYPEMASK_VX_IGNORE |
        POSITION_TARGET_TYPEMASK_VY_IGNORE |
        POSITION_TARGET_TYPEMASK_VZ_IGNORE;

    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE =
        POSITION_TARGET_TYPEMASK_AX_IGNORE |
        POSITION_TARGET_TYPEMASK_AY_IGNORE |
        POSITION_TARGET_TYPEMASK_AZ_IGNORE;

    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE =
        POSITION_TARGET_TYPEMASK_YAW_IGNORE;
    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE =
        POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
    constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_FORCE_SET =
        POSITION_TARGET_TYPEMASK_FORCE_SET;

    switch (msg.msgid) {
	case MAVLINK_MSG_ID_MANUAL_CONTROL:
    {
        if (msg.sysid != copter.g.sysid_my_gcs) {
            break; // only accept control from our gcs
        }

        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(&msg, &packet);

        if (packet.target != copter.g.sysid_this_mav) {
            break; // only accept control aimed at us
        }

        if (packet.z < 0) { // Copter doesn't do negative thrust
            break;
        }

        uint32_t tnow = AP_HAL::millis();

        manual_override(copter.channel_roll, packet.y, 1000, 2000, tnow);
        manual_override(copter.channel_pitch, packet.x, 1000, 2000, tnow, true);
        manual_override(copter.channel_throttle, packet.z, 0, 1000, tnow);
        manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow);

        // a manual control message is considered to be a 'heartbeat'
        // from the ground station for failsafe purposes
        gcs().sysid_myggcs_seen(tnow);
        break;
    }
    ...

2.发送

(1)
在\libraries\GCS_MAVLink\GCS_Common.cpp中
void GCS::update_send()
对于update_send（）进行介绍。定位到chan(i)->update_send()函数，该语句实现对于每一个通道上的MAVLINK消息的发送。在同一文件下的 void GCS_MAVLINK::update_send()函数中，找到do_try_send_message()，其是最主要的功能部分实现。

void GCS::update_send()
{
    update_send_has_been_called = true;
#ifndef HAL_BUILD_AP_PERIPH
    if (!initialised_missionitemprotocol_objects) {
        initialised_missionitemprotocol_objects = true;
        // once-only initialisation of MissionItemProtocol objects:
        AP_Mission *mission = AP::mission();
        if (mission != nullptr) {
            _missionitemprotocol_waypoints = new MissionItemProtocol_Waypoints(*mission);
        }
        AP_Rally *rally = AP::rally();
        if (rally != nullptr) {
            _missionitemprotocol_rally = new MissionItemProtocol_Rally(*rally);
        }
        AC_Fence *fence = AP::fence();
        if (fence != nullptr) {
            _missionitemprotocol_fence = new MissionItemProtocol_Fence(*fence);
        }
    }
    if (_missionitemprotocol_waypoints != nullptr) {
        _missionitemprotocol_waypoints->update();
    }
    if (_missionitemprotocol_rally != nullptr) {
        _missionitemprotocol_rally->update();
    }
    if (_missionitemprotocol_fence != nullptr) {
        _missionitemprotocol_fence->update();
    }
    #endif // HAL_BUILD_AP_PERIPH
    // round-robin the GCS_MAVLINK backend that gets to go first so
    // one backend doesn't monopolise all of the time allowed for sending
    // messages
    for (uint8_t i=first_backend_to_send; i<num_gcs(); i++) {
        chan(i)->update_send();
    }
    for (uint8_t i=0; i<first_backend_to_send; i++) {
        chan(i)->update_send();
    }

    service_statustext();

    first_backend_to_send++;
    if (first_backend_to_send >= num_gcs()) {
        first_backend_to_send = 0;
    }
}

(2)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS_MAVLINK::update_send()函数中，找到do_try_send_message()，其是最主要的功能部分实现。

void GCS_MAVLINK::update_send()
{
    ...
    const int8_t next = deferred_message_to_send_index(start16);
    if (next != -1) {
        if (!do_try_send_message(deferred_message[next].id)) {
            break;
        }
        ...
    ]
   ...
}

(3)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

do_try_send_message函数中，会在合适的时间调用try_send_message()函数，这也是do_try_send_message()函数的最重要的功能。

bool GCS_MAVLINK::do_try_send_message(const ap_message id)
{
    ...
    if (!try_send_message(id)) {
   ...
}

(4)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

try_send_message()函数内部同样使用了switch-case语句通过判断具体的MAVLINK消息id来实现对于不同APM系统信息的发送功能，并返回布尔类型的值，发送成功即返回true。
以MSG_ATTITUDE消息为例，该case语句中首先对PAYLOAD长度进行判断，长度合适返回true。然后调用具体的消息处理函数。

bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
    bool ret = true;

    switch(id) {

    case MSG_ATTITUDE:
        CHECK_PAYLOAD_SIZE(ATTITUDE);
        send_attitude();
        break;

    case MSG_ATTITUDE_QUATERNION:
        CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION);
        send_attitude_quaternion();
        break;

    case MSG_NEXT_PARAM:
        CHECK_PAYLOAD_SIZE(PARAM_VALUE);
        queued_param_send();
        break;
    ...
}

(5)
在\libraries\GCS_MAVLink\GCS_Common.cpp中

void GCS_MAVLINK::send_attitude() const

这个函数（其他消息也类似）会在内部调用由消息头文件所提供的函数接口，用来实现MAVLINK消息的发送功能。

这个函数实现在对应消息的头文件中，通过将需要传输的信息生成buf或者packet（究竟是哪个就需要看协议是怎么定的啦，可以全局找一下protocol.h这个文件），然后通过_mav_finalize_message_chan_send()函数发送出去。

void GCS_MAVLINK::send_attitude() const
{
    const AP_AHRS &ahrs = AP::ahrs();
    const Vector3f omega = ahrs.get_gyro();
    mavlink_msg_attitude_send(
        chan,
        AP_HAL::millis(),
        ahrs.roll,
        ahrs.pitch,
        ahrs.yaw,
        omega.x,
        omega.y,
        omega.z);
}

(6)
在\libraries\GCS_MAVLink\include\mavlink\v2.0\common\mavlink_msg_attitude.h中

mavlink_msg_attitude_send函数

static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char buf[MAVLINK_MSG_ID_ATTITUDE_LEN];
    _mav_put_uint32_t(buf, 0, time_boot_ms);
    _mav_put_float(buf, 4, roll);
    _mav_put_float(buf, 8, pitch);
    _mav_put_float(buf, 12, yaw);
    _mav_put_float(buf, 16, rollspeed);
    _mav_put_float(buf, 20, pitchspeed);
    _mav_put_float(buf, 24, yawspeed);

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
#else
    mavlink_attitude_t packet;
    packet.time_boot_ms = time_boot_ms;
    packet.roll = roll;
    packet.pitch = pitch;
    packet.yaw = yaw;
    packet.rollspeed = rollspeed;
    packet.pitchspeed = pitchspeed;
    packet.yawspeed = yawspeed;

    _mav_final

总结

以上就是今天要讲的内容，介绍了MAVLINK协议相关内容。

axzg
关注
深入解析MAVLink 1协议在STM32上的应用：从外部命令到自定义数据处理的完整实现指南
qq_38334677的博客
09-03 848
在无人机和机器人系统的开发过程中，通信协议的选择至关重要。MAVLink（Micro Air Vehicle Link）是广泛应用于无人机和机器人系统的轻量级通信协议，特别是在飞行控制系统和地面站之间的数据传输中。它提供了可靠且高效的数据交换机制，用于传输位置、姿态、传感器读数等信息，并允许远程操控设备执行复杂的任务。尽管MAVLink 2已经发布，但MAVLink 1仍然在许多项目中得到广泛支持。
Ardupilot 增加一个mavlink消息（翻译官网）
陌城烟雨
09-10 1971
目录 目录 摘要 第一： 翻译官网资料 增加一条新的mavlink消息（Adding a new MAVLink Message） 摘要 本节主要记录自己翻译ardupilot增加一条新的mavlink消息的过程，如果有错误的欢迎批评指正。微信：lxw15982962929或者手机：18129927205，扣扣：995439743 第一： 翻译官网资料 ...
MAVLink通讯协议全文
07-10
MAVLink通讯协议全文，中文详解
MAVLink 通信协议简介：无人机与机器人系统的轻量级解决方案
最新发布
weixin_41417788的博客
08-15 790
MAVLink 是一种轻量级、二进制的消息传输协议，广泛应用于无人机和机器人系统中，用于系统与控制台或地面站之间的通信。MAVLink 的全称是 Micro Air Vehicle Link，它最早由 Lorenz Meier 在 2009 年开发，并逐渐成为开源无人机控制软件如 PX4 和 ArduPilot 的标准通信...
Mavlink协议
pengl的博客
09-21 2万+
Mavlink协议简介Mavlink协议简介 一Mavlink协议 二数据结构 三协议支持的数据类型 四性能 五航点协议 读取MAV的航点列表 写MAV的航点列表 清除MAV的航点列表 设置当前MAV航点 六参数的读写 读取参数列表 读取单个参数 写参数 七增加新的mavlink消息 XML文件中消息的定义 建立消息定义文件 编绎XML文件 八消息的发送与解析 九通用的Mavlink消息 参考资料
ardupilot开发 --- MavLink 篇
记录老板的每次pua，分享领导每天的毒鸡汤...
08-15 1984
MavLink文档阅读笔记
MAVLink通讯协议全文.pdf
07-09
MAVLink（Micro Air Vehicle Link，微型空中飞行器链路通讯协议）是无人飞行器与地面站（Ground Control Station ，GCS）之间通讯，以及无人飞行器之间通讯最常用的协议。它已经在PX4、APM、PIXHAWK和Parrot AR.Drone飞控平台上进行了大量测试。 这是中文翻译文档
一文搞定px4、ardupilot mavlink软件协议
LINGTUO_TBUS的博客
01-14 1348
搞开源无人机的朋友最耳熟能详的莫过于它的通讯协议MAVLINK了。 Mavlink----一个又好气又好笑的名词，仿佛自带光环，它一出场，就会附带两个小弟：ros、mavros。 网络上mavros免费的课程一大堆，mavlink的倒是少之又少。 很多人都会形成一种观念，难道是因为mavros要简单些？今天L君要告诉你们，其实直接使用mavlink比使用mavros简单的多啦。 对于一个老程序员来说，理解数据结构犹如吃饭，但是理解框架犹如登天！ 先来好好说说这个大哥和两小弟的关系。 ROS robot o
Ardupilot飞控Mavlink代码学习
陌城烟雨
07-05 7393
目录 目录 摘要 1.Ardupilot怎么实现Mavlink初始化 2.Mavlink消息通信过程 摘要 本节主要记录自己学习Ardupilot的Mavlink协议的过程，欢迎一起交流分析！ 1.Ardupilot怎么实现Mavlink初始化 （1）初始化USB端口，进行Mavlink通信 可以看出gcs_chan是GCS_MAV...
【无人机开发】通讯协议MavLink详解
热门推荐
宝爷的专栏
07-19 4万+
1. MAVLink简介 MAVLink（Micro Air Vehicle Link，微型空中飞行器链路通讯协议）是无人飞行器与地面站（Ground Control Station ，GCS）之间通讯，以及无人飞行器之间通讯最常用的协议。它已经在PX4、APM、PIXHAWK和Parrot AR.Drone飞控平台上进行了大量测试。 2.发明者Lorenz Meier简介 MAVLink的...
MAVLink通信协议
04-23
mavlink 地面控制集成教程适应QGroundControl，Mavlink 编码和驱动，mavlink 消息定义和驱动代码托管，mavlink 的Python 绑定等的介绍！
mavlink通讯协议
12-03
mavlink通讯协议
MAVLink微型飞行器的通信协议.pdf
09-18
MAVLink微型飞行器的通信协议,适用于MP空间地面站。
MAVLink V1.0通讯协议全文.pdf
03-12
MAVLink V1.0 通信协议全文，MAVLink全称Micro Air Vehicle Link ，即微型飞行器连接通信协议，于2009年初由Lorenz Meier首次发布。
MAVLINK协议
weixin_30435261的博客
09-16 232
参考https://cloud.tencent.com/developer/news/48344 https://www.cnblogs.com/lovechen/p/5801679.html https://www.cnblogs.com/lovechen/p/5809709.htm https://www.jianshu.com/p/e57aa664103f https://...
MavLinK协议
全栈技术分享，项目分享，资料分享
05-19 1324
MAVSDK是PX4开源团队贡献的基于mavlink通信协议的用于无人机应用开发的SDK，其可以部署在Windows、Linux、Android等多种平台，并且支持多种语言如c/c++、python、Java等。在官网你会看到不同语言的MAVSDK，对应新手来说可能有点懵，其实MAVSDK的核心是MAVSDK-C++，这个版本实现了包括接口驱动（串口、udp等）、mavlink通信、各种交互逻辑处理，并且创建了server，建立gRPC通信，其他不同的语言版本都是只实现了gRPC通信接口而已。
Ardupilot自定义mavlink消息
PENG__WANG的博客
07-28 975
ardupilot自定义mavlink消息
MAVLink 飞控通讯协议开发详解
zhoubiaodi的博客
04-29 5493
介绍无人机和地面站之间的通讯协议（MAVLink飞控通讯协议）、开发流程、报文格式和通用消息集，方便操控无人机。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值