程序员经验分享-hwhale

MAVLINK消息在Ardupilot中的接收和发送过程

2023-05-16

MAVLINK消息在Ardupilot中的接收和发送过程

  • SCHED_TASK
  • update_receive()
  • update_send()

由于现在网上很多的都是APM旧版本的解释，因此把自己的一些学习所得记录下来。截至写博客日期（2020/10/21），本文APM版本为4.0（针对各种车辆的）。

SCHED_TASK

关于MAVLINK消息在APM中的初始化过程，在之前的博文里面已经写过了：MAVLINK在Ardupilot中的初始化过程。建议先阅读一下，虽然写得不是很完整，但还是能理一下思路的，后面有时间的话会进一步更新修改。

打开Ardupilot源码项目，进入随机车辆类型的主目录文件下找到对应的主程序文件，如在ArduCopter路径下，Copter.cpp中，对应文件下始终实现了两个函数的任务调度器。

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

又如在ArduSub文件夹下的ArduSub.cpp文件中，同样实现了

SCHED_TASK_CLASS(GCS,                 (GCS*)&sub._gcs,   update_receive,     400, 180),
SCHED_TASK_CLASS(GCS,                 (GCS*)&sub._gcs,   update_send,        400, 550),

这两个函数始终以400Hz的频率运行，在APM上进行MAVLINK消息的发送和接收。

update_receive()

这个函数实现在ardupilot/libraries/GCS_MAVLINK/GCS_Common.cpp文件中。在之前的博文里面我们说过，GCS类相当于是在APM中实现了一个“地面站”的功能，用于发送和接收消息（当然并不完全正确）。具体来看里面的内容：

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

虽然这个函数在之前已经介绍过了，这边还是再说一下。num_gcs()获取到总共的MAVLINK数据传输接口（或链路）。然后对于每一个通道执行更新接收操作update_receive()。这个函数才是实际上对于每一个通道接口真正执行接收MAVLINK消息的函数。

这部分代码有点长就不全部放上来了，实际上对于MAVLINK接收消息并且解包的过程之前也有博文讲过（重读Ardupilot中stabilize model+MAVLINK解包过程），但是前面的一些知识点还是比较零散的，这边还是再汇总一下。

还是挑重点说一下，首先是在原函数中找到该段

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

其他的可以先不追究细节，重点找到里面的

packetReceived(status, msg);

进入该函数

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) {
        const uint8_t mask = (1U<<(chan-MAVLINK_COMM_0));
        if (!(mask & mavlink_private)) {
            mavlink_active |= mask;
        }
    }
    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 (!accept_packet(status, msg)) {
        // e.g. enforce-sysid says we shouldn't look at this packet
        return;
    }
    handleMessage(msg);
}

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

定位到该函数定义于库内的GCS.h文件中

virtual void        handleMessage(const mavlink_message_t &msg) = 0;

在GCS_MAVLINK类中实现为一个虚函数，因此我们必须在各个具体的车辆类型中找到对应派生类中的具体实现。

以ArduSub为例，在上层车辆文件夹中的GCS_Mavlink.h中实现了GCS_MAVLINK_Sub类对于GCS_MAVLINK消息的继承，并且在GCS_Mavlink.cpp文件中实现了对于handleMessage()的重写。

void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
{
    switch (msg.msgid) {

    case MAVLINK_MSG_ID_HEARTBEAT: {    // MAV ID: 0
        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
        if (msg.sysid != sub.g.sysid_my_gcs) {
            break;
        }
        sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
        break;
    }

	case ...
	...
}

可以看到在内部通过switch-case语句实现了根据具体的mavlink消息id进行不同的事件处理的功能（内容太长就不全部放上来了）。

关于update_receive()函数了解到这里就差不多了。

update_send()

重新回到ArduSub.cpp中，我们对于update_send（）进行介绍。

同样在ardupilot/libraries/GCS_MAVLINK/GCS_Common.cpp文件中

void GCS::update_send()
{
    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);
        }
    }
    if (_missionitemprotocol_waypoints != nullptr) {
        _missionitemprotocol_waypoints->update();
    }
    if (_missionitemprotocol_rally != nullptr) {
        _missionitemprotocol_rally->update();
    }
    for (uint8_t i=0; i<num_gcs(); i++) {
        chan(i)->update_send();
    }
    WITH_SEMAPHORE(_statustext_sem);
    service_statustext();
}

多余部分不再过多讲解，定位到chan(i)->update_send()函数，该语句实现对于每一个通道上的MAVLINK消息的发送。在同一文件下的 void GCS_MAVLINK::update_send()函数中（内容过长不放上来了），找到do_try_send_message(),其是最主要的功能部分实现。

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

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

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

这个函数（其他消息也类似）会在内部调用由消息头文件所提供的函数接口，用来实现MAVLINK消息的发送功能（不理解的同学看这边：QGC添加自定义组件和发送自定义MAVLINK消息，这篇博文的第二章）。

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

关于这部分内容，了解这么多应该就足够了。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

MAVLink

ArduPilot

中的接收和发送过程

MAVLINK消息在Ardupilot中的接收和发送过程 的相关文章

  • 重读Ardupilot中stabilize model+MAVLINK解包过程

    APM源码和MAVLINK解析学习 重读stabilize stabilize modelinit run handle attitude MAVLINK消息包姿态信息传输过程 之前写的模式都是基于master版本的 xff0c 这次重读s

  • QGC添加自定义组件和发送自定义MAVLINK消息

    QGC添加自定义组件和发送自定义MAVLINK消息 一 添加自定义组件1 1 在飞行界面添加组件1 2 实现组件事件1 3 在MOCK模拟链接中实现验证1 4 验证 二 自定义MAVLINK消息的一些预备知识三 QGC自定义MAVLINK消

  • Ardupilot速率控制器rate_controller_run解析

    Ardupilot速率控制器rate controller run解析 PID速率控制器源码解析rate controller run PID运算积分限制update i get ff set xxx 内容补充 xff1a 函数中陀螺仪数据

  • ArduPilot飞行前检查——PreArm解析

    ArduPilot飞行前检查 主要包括两个部分 1 初始化中遥控器输入检查 xff1b 2 1Hz解锁前检查 附 xff1a 显示地面站信息 参考文章 xff1a Ardupilot Pre Arm安全检查程序分析 1 初始化中遥控器输入检

  • matlab发送mavlink消息

    主要介绍了通过matlab脚本实现UDP发送mavlink消息 xff0c 为后面matlab计算 xff0c 与Optitrack联合调试 xff0c 控制无人机做准备 示例演示效果链接为 matlab通过UDP协议发送mavlink消息

  • ArduPilot 添加自定义调节参数

    实际变成操作中 xff0c 需要对ardupilot代码进行修改并添加对应的调试参数 xff0c 这样 xff0c 可以通过地面站很方便的进行修改参数 目前修改代码在parameter h中的G2类 xff0c 表示为全局的参数列表 参数类

  • Pixhawk之UAV控制理论、ardupilot源码框架介绍

    一 开篇 您有无人机么 xff1f 没有 那赶紧去某宝买一套 昨天 开会开到接近下午一点钟 xff0c 收获相当大 xff0c 原本不太清楚的ardupilot框架现在也大致熟悉了 xff0c 接下来主要就是结合源码了解其控制过程了 xff

  • ADRC Ardupilot代码分析

    记录一下自己对于Ardupilot ADRC控制代码的一些理解 GitHub链接 ADRC Active Disturbance Rejection Control by MichelleRos Pull Request 20243 Ard

  • px4中的mavlink协议

    原文地址 xff1a http blog csdn net oqqENvY12 article details 61615609 PX4 对Mavlink 协议提供了良好的原生支持 该协议既可以用于地面站 Ground ControlSta

  • ArduPilot日志系统探索（一）

    先把官方网站上日志相关的说明翻译下来 xff1a ArduPilot Documentation ArduPilot documentation 页面 xff1a Logs Copter documentation 与日志记录和分析相关的主

  • PX4自定义Mavlink消息（一）px4发送

    用于项目需求 xff0c 需要在PX4和QGC新增自定义消息用于通信控制 xff0c 看了很多有关文章 xff0c 我就不明白 xff0c 为啥代码都是复制来复制去的 xff0c 大部分的文章连变量名的起的一样 一点帮助都没有 xff0c

  • MAVLink.io(4)--MAVLlink Version

    MAVLink Version 版本 MAVLink发展处几个版本 xff1a MAVLink 2 0 目前推荐的主要版本 xff0c 2017被大部分用户接受 MAVLink v1 0 2013年被广泛接受 xff0c 一直被大批设备采用

  • QuadPlane (VTOL)——ArduPilot——流程梳理

    版权声明 xff1a 本文为博主原创博文 xff0c 未经允许不得转载 xff0c 若要转载 xff0c 请说明出处并给出博文链接

  • 【2020-8-9】APM,PX4,GAZEBO,MAVLINK,MAVROS,ROS之间的关系以及科研设备选型

    0 概述 无人机自主飞行平台可以分为四个部分 xff1a 动力平台 xff0c 飞行控制器 xff0c 机载电脑和模拟平台 动力平台 xff1a 负责执行飞行任务 xff0c 包括螺旋桨 电机 机架等 xff0c 用于科研的一般都是F380

  • APM 学习 6 --- ArduPilot 线程

    ArduPilot 学习之路 6 xff0c 线程 英文原文地址 xff1a https ardupilot org dev docs learning ardupilot threading html 理解 ArduPilot 线程 线程

  • PX4模块设计之五：自定义MAVLink消息

    PX4模块设计之五 xff1a 自定义MAVLink消息 1 MAVLink Dialects1 1 PX4 Dialects1 2 Paprazzi Dialects1 3 MAVLink XML File Format 2 添加自定义M

  • mavlink python

    from pymavlink import mavutil Create the connection m 61 mavutil mavlink connection 39 udpin 0 0 0 0 14550 39 dir m mav

  • SITL Simulator —— ArduPilot —— Windows

    版权声明 xff1a 本文为博主原创博文 xff0c 未经允许不得转载 xff0c 若要转载 xff0c 请说明出处并给出博文链接 参考网页 xff1a http ardupilot org dev docs sitl native on

  • PX4中自定义MAVLink消息（记录）

    简单记录一下这个过程 一 自定义uORB消息 这一步比较简单 xff0c 首先在msg 中新建ca trajectory msg文件 uint64 timestamp time since system start span class t

  • ArduPilot/APM源码学习笔记（一）

    最近开始学习ArduPilot APM飞控的源码 xff0c 源码托管在github上 源码链接 xff1a https github com diydrones ardupilot 飞控主页 xff1a http ardupilot co

随机推荐

  • 头文件与库的区别

    昨天突然问了一下什么是头文件 xff0c 我一听就傻了 xff0c 虽然上课的时候老师在讲编译的四个过程的时候说了一下 xff0c 但是还是不太理解 xff0c 我们知道编译过程中的预处理阶段会进行头文件展开 xff0c 宏替换以及条件编译

  • 进程、线程

    线程 xff08 thread xff09 线程其实是操作系统能够进行运算调度的最小单位 它是被包含在进程之中的 xff0c 是进程中的实际运作单位 一条线程指的是进程中一个单一顺序的控制流 xff0c 一个进程中可以并发多个线程 xff0

  • 基于Zynq7020双千兆以太网的数字信号处理板设计

    一 背景 背景 Xilinx公司在2010年发布了可扩展的处理器平台Zynq7000系列 xff0c 它采用了28nm工艺 xff0c 将FPGA与ARM cortex A9集成在一颗芯片上 xff0c 实现了高性能 高集成度 低功耗 Zy

  • 深入理解JS中的变量作用域

    在 JS 当中一个变量的作用域 xff08 scope xff09 是程序中定义这个变量的区域 变量分为两类 xff1a 全局 xff08 global xff09 的和局部的 其中全局变量的作用域是全局性的 xff0c 即在 JavaSc

  • 硬件工程师，从零开始无人机开发。

    毕业已经五年了 xff0c 一直在杭州某大厂 xff0c 做无人机硬件开发 无人机这块 xff0c 我进去的时候大厂刚开始 做 xff0c 有幸参与到整个无人机的硬件开发 我这个刚毕业的技术小白 xff0c 在这五年间成长了很多 无奈 今年

  • 个人总结：板球控制系统之串级PID整定方法，速度环与位置环，40S任务10S完成

    其实单环我们先出了所有题目 xff0c 但是效果显然没有串级PID的效果好 xff0c 有人需要的话可以把程序包发出来 xff0c 板球运行视屏也有 另外 xff1a 天下舵机参差不齐 xff08 哪怕型号相同 xff09 xff0c 想要

  • 树莓派3B+踩坑记录：一、安装Ubuntu Mate

    树莓派3B 43 踩坑记录 xff1a 一 安装Ubuntu Mate 树莓派 xff0c Ubuntu xff0c ROS硬件准备软件准备系统烧录安装Ubuntu Mate更换国内源网络配置开启ssh远程其他彩虹屏解决方案XShell和X

  • PointNet代码详解

    PointNet代码详解 最近在做点云深度学习的机器人抓取 xff0c 这篇博客主要是把近期学习PointNet的一些总结的知识点汇总一下 PointNet概述详见以下网址和博客 xff0c 这里也就不再赘述了 三维深度学习之pointne

  • 卡尔曼滤波通俗易懂的解释

    关于卡尔曼滤波 xff0c 网上的资料很多 xff0c 但是有很大一部分都是不断堆叠公式 xff0c 然后用各种晦涩难懂的专业术语进行解释 xff0c 说实话我刚开始看的时候也是云里雾里 xff0c 因此写下这篇博客是为了照顾和我一样的萌新

  • STM32通过PWM控制ESC30C电调

    最近在搞一个水下推进器 xff0c 这东西的控制其实跟四旋翼的螺旋桨控制差不多 但我也是第一次用STM32板子来控制电调驱动桨叶旋转 xff0c 因此踩了很多坑 网上找了很多资料 xff0c 但是很多都写的不是很清楚 xff0c 这边稍微记

  • STM32F7同一定时器多路输出PWM波通道之间相互影响问题

    2020 8 12更新 这次用Cube直接生成PWM控制代码 xff0c 然后再RT Thread Studio上编写程序 xff0c 发现可实现TIM1和TIM8的8路PWM波调控 xff0c 因此上面论述的问题可能是自己在写底层时有某些

  • Ardusub源码解析学习（一）——Ardusub主程序

    APM Sub源码解析学习 xff08 一 xff09 Ardusub主程序 前言一 准备工作二 Ardusub cpp解析2 1 scheduler table2 2 schedulerget scheduler tasks setup

  • Ardusub源码解析学习（二）——电机库

    Ardusub源码解析学习 xff08 二 xff09 电机库学习 一 RC输入与输出1 1 RC Input1 2 RC Output 二 电机库学习2 1 setup motors 2 2 add motor raw 6dof 2 3

  • Ardusub源码解析学习（三）——车辆类型

    APM Sub源码解析学习 xff08 三 xff09 车辆类型 一 前言二 class AP HAL HAL三 class AP Vehicle3 1 h3 2 cpp 四 class Sub4 1 h4 2 cpp 五 总结 一 前言

  • 年度回忆录（2012.10----2013.01）

    寒假结束了 xff0c 年也过完了 xff0c 提前回来一天就开始着手补上这迟到的年终总结 xff0c 写了一个多星期还觉得有些东西没有写出来 xff0c 无奈 xff0c 点到为止吧 2012 年的后半年经历了很多 xff0c 收获了很多

  • Ardusub学习——飞行模式

    参考资料 xff1a Ardusb官方手册 Sub Rework joystick input and pilot input in general Flight Modes Ardusub支持多种飞行模式 xff0c 但是其中一部分需要有

  • Ardusub源码解析学习（五）——从manual model开始

    Ardusub源码解析学习 xff08 五 xff09 从manual model开始 manual init manual run 从本篇开始 xff0c 将会陆续对Ardusub中各种模式进行介绍 xff0c stabilize mod

  • 重读Ardupilot中stabilize model+MAVLINK解包过程

    APM源码和MAVLINK解析学习 重读stabilize stabilize modelinit run handle attitude MAVLINK消息包姿态信息传输过程 之前写的模式都是基于master版本的 xff0c 这次重读s

  • QGC添加自定义组件和发送自定义MAVLINK消息

    QGC添加自定义组件和发送自定义MAVLINK消息 一 添加自定义组件1 1 在飞行界面添加组件1 2 实现组件事件1 3 在MOCK模拟链接中实现验证1 4 验证 二 自定义MAVLINK消息的一些预备知识三 QGC自定义MAVLINK消

  • MAVLINK消息在Ardupilot中的接收和发送过程

    MAVLINK消息在Ardupilot中的接收和发送过程 SCHED TASKupdate receive update send 由于现在网上很多的都是APM旧版本的解释 xff0c 因此把自己的一些学习所得记录下来 截至写博客日期 xf

热门标签