QGC关于SetMode运行流程解析

QGC与飞控连接后初始化，初始飞行模式为手动模式

模式切换在Vehicle中使用_base_mode和_custom_mode存储模式，初始值均为0。

当QGC与飞控连接后，飞控通过心跳包（heartbeat）向下发送base_mode和custom_mode,在心跳包中更新_base_mode和custom_mode的值，具体更细在心跳包数据解析中更新。
关于base_mode,是指飞控现在处在哪个基本模式，对于发心跳包的地面站来说没有意义，对于发送心跳包的飞控来说是有意义的。这个参数要看各个飞控自己的定义方式，mavlink介绍网页并不会给出具体的模式。在Pixhawk中基本模式可以分为使用用户模式（custom mode）还是基本模式（这里有点绕，其实是就是是否使用用户模式）。使用基本模式又会分为自动模式（auto），位置控制模式（posctl）和手动模式（manual）。一般情况下都会使用用户模式，普通用户不用关心这个参数。开发者在使用mavlink修改飞行器模式时需要注意基本模式的设置。
关于custom_mode,大概说一下Pixhawk的用户模式。以多轴为例。它分为主模式（main mode）和子模式（sub mode），两种模式组合在一起成为最终的模式，主模式分为3种，手动（manual），辅助（assist），自动（auto）。手动模式类似apm的姿态模式。在辅助模式中，又分为高度控制模式（altctl）和位置控制模式（posctl）两个子模式，高度控制模式就类似apm的定高模式，油门对应到飞行器高度控制上。位置模式控制飞行器相对地面的速度，油门和高度控制模式一样，yaw轴控制和手动模式一样。自动模式里又分为3个子模式，任务模式（mission），留待模式（loiter），返航模式（return），任务模式就是执行设定好的航点任务，留待模式就是gps悬停模式，返航模式就是直线返回home点并自动降落。在apm里这个参数貌似是没有用的，注意这个数据占了4个字节，在Pixhawk中，前两个字节（低位）是保留的，没有用，第三个字节是主模式，第四个字节是子模式。普通用户请无视。custom_mode又分为main_mode和sub_mode,三者关系如下：

uint32 custom_mode = (main_mode<<16)|(sub_mode<<24);

下边是custom_mode中飞行模式对应的main_mode和sub_mode:

    static const struct Modes2Name rgModes2Name[] = {//main_mode                         sub_mode                                canBeSet  FW      MC{ PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                      true,   false,  true },{ PX4_CUSTOM_MAIN_MODE_SIMPLE,      0,                                      false,  false,  true },{ PX4_CUSTOM_MAIN_MODE_POSCTL,      PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL,      true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_POSCTL,      PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT,       false,  false,   false },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,        true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,       true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,           true,   true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, true,   false,  true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,          false,  true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,      false,  false,  true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,         false,  true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,          false,  true,   true },{ PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,       false,  true,   true },};

if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {QString previousFlightMode;if (_base_mode != 0 || _custom_mode != 0){// Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about// bad modes while unit testing.previousFlightMode = flightMode();}_base_mode   = heartbeat.base_mode;_custom_mode = heartbeat.custom_mode;if (previousFlightMode != flightMode()) {emit flightModeChanged(flightMode());}}

在飞控中初始为手动模式,初始值如下所示：

base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED =0b00000001;custom_mode = PX4_CUSTOM_MAIN_MODE_MANUAL << 16 = 0b00000001;

飞行模式切换

QGC飞行模式切换功能，在ModeIndicator.qml进行显示且通过下拉框点击触发，下面对整个触发流程进行分析。

前端触发，通过前端点击触发功能。当鼠标点击后，触发onActivated中将切换的飞行名称赋值给 _activeVehicle.flightMode。

QGCComboBox {anchors.verticalCenter: parent.verticalCenteralternateText:          _activeVehicle ? _activeVehicle.flightMode : ""model:                  _flightModesfont.pointSize:         ScreenTools.mediumFontPointSizecurrentIndex:           -1sizeToContents:         trueproperty bool showIndicator: trueproperty var _activeVehicle:    QGroundControl.multiVehicleManager.activeVehicleproperty var _flightModes:      _activeVehicle ? _activeVehicle.flightModes : [ ]onActivated: {_activeVehicle.flightMode = _flightModes[index]currentIndex = -1}
}

在Vehicle.h中对flightMode使用Q_PROPERTY设置了读写属性，在前端点击切换模式后，使flightMode属性改变，便会触发setFlightMode()函数执行。

Q_PROPERTY(QString              flightMode              READ flightMode             WRITE setFlightMode             NOTIFY flightModeChanged)

在setFlightMode()中根据当前飞行模式flightMode使用_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)更新base_mode和custom_mode值，并通过mavlink协议发送mavlink_msg_set_mode_pack_chan()，将切换飞行指令发送到飞控。

void Vehicle::setFlightMode(const QString& flightMode)
{uint8_t     base_mode;uint32_t    custom_mode;if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {// setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing// states.uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE;newBaseMode |= base_mode;mavlink_message_t msg;mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),_mavlink->getComponentId(),priorityLink()->mavlinkChannel(),&msg,id(),newBaseMode,custom_mode);sendMessageOnLink(priorityLink(), msg);} else {qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;}
}

bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{*base_mode = 0;*custom_mode = 0;bool found = false;foreach (const FlightModeInfo_t& info, _flightModeInfoList) {if (flightMode.compare(info.name, Qt::CaseInsensitive) == 0) {union px4_custom_mode px4_mode;px4_mode.data = 0;px4_mode.main_mode = info.main_mode;px4_mode.sub_mode = info.sub_mode;*base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;*custom_mode = px4_mode.data;found = true;break;}}if (!found) {qWarning() << "Unknown flight Mode" << flightMode;}return found;
}

当飞控接收到mavlink协议，对飞行模式指令进行解析mavlink_msg_set_mode_decode，解析后切换飞行模式并更新飞控端base_mode和custom_mode值，通过心跳包发送到QGC。（此时QGC收到的base_mode和custom_mode为新的飞行模式对应的值）

uint32 custom_mode = 0;switch (mode_cmd) {case GCS_CMD_MANUAL:custom_mode = CEVT_CUSTOM_MAIN_MODE_MANUAL << 16;break;case GCS_CMD_MISSION:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_MISSION << 24);break;case GCS_CMD_RTL:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_RTL << 24);break;case GCS_CMD_SAFELAND:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_SAFELAND << 24);break;case GCS_CMD_TAKEOFF:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_TAKEOFF << 24);break;case GCS_CMD_LAND:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_LAND << 24);break;case GCS_CMD_PRECLAND:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_PRECLAND << 24);break;case GCS_CMD_LOITER:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_LOITER << 24);break;case GCS_CMD_ALT:custom_mode = (CEVT_CUSTOM_MAIN_MODE_AUTO << 16) + (CEVT_CUSTOM_SUB_MODE_AUTO_ALTITUDE << 24);break;default:custom_mode = 0;}

在Vehicle::_handleHeartbeat()中，将飞控下发的base_mode和custom_mode赋值给_base_mode和_custom_mode，并且使用_base_mode和_custom_mode通过flightMode()函数查询对应的飞行模式与previousFlightMode比较，当不一致时则触发emit flightModeChanged(flightMode());

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{if (message.compid != _defaultComponentId) {return;}mavlink_heartbeat_t heartbeat;mavlink_msg_heartbeat_decode(&message, &heartbeat);bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;// ArduPilot firmare has a strange case when ARMING_REQUIRE=0. This means the vehicle is always armed but the motors are not// really powered up until the safety button is pressed. Because of this we can't depend on the heartbeat to tell us the true// armed (and dangerous) state. We must instead rely on SYS_STATUS telling us that the motors are enabled.if (apmFirmware()) {if (!_apmArmingNotRequired() || !(_onboardControlSensorsPresent & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)) {// If ARMING_REQUIRE!=0 or we haven't seen motor output status yet we use the hearbeat info for armed_updateArmed(newArmed);}} else {// Non-ArduPilot always updates from armed state in heartbeat_updateArmed(newArmed);}if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {QString previousFlightMode;if (_base_mode != 0 || _custom_mode != 0){// Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about// bad modes while unit testing.previousFlightMode = flightMode();}_base_mode   = heartbeat.base_mode;_custom_mode = heartbeat.custom_mode;if (previousFlightMode != flightMode()) {emit flightModeChanged(flightMode());}}
}

当触发flightModeChanged()后，ModeIndicator.qml会刷新alternateText值。
在Vehicle.cc还设置了信号槽，当flightModeChanged触发后，_handleFlightModeChanged执行。

connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);

在_handleFlightModeChanged()中首先进行语音播报，然后发送guideModeChanged信号。

void Vehicle::_handleFlightModeChanged(const QString& flightMode)
{_say(tr("%1 %2 flight mode").arg(_vehicleIdSpeech()).arg(flightMode));emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
}

这一块个人感觉是在guide mode下才会使用，这一块逻辑不是太清晰。个人感觉模式设置这一块在完成语音播报后就结束了。

Q_PROPERTY(bool     guidedMode              READ guidedMode                 WRITE setGuidedMode NOTIFY guidedModeChanged)///< Vehicle is in Guided mode and can respond to guided commands

QGC关于SetMode运行流程解析相关推荐

Spark SQL之queryExecution运行流程解析Logical Plan（三）
1.整体运行流程使用下列代码对SparkSQL流程进行分析,让大家明白LogicalPlan的几种状态,理解SparkSQL整体执行流程 // sc is an existing SparkCont ...
UcosII移植、调度、功能、运行流程解析
吐槽下csdn的编辑功能,真的菜的抠脚. 接下来,陆陆续续会将语雀的个人笔记搬移到csdn上. 该篇文档过长,建议目录查找关键位置. UcosII是剥夺内核(每个系统节拍会切换到最高就绪优先级任务), ...
pyspider创建淘女郎图片爬虫任务-运行流程解析
单步运行运行该任务,第一步爬取多页: 每个页面里面抓取10个淘女郎主页链接: 对每个淘女郎主页详情页面爬取图片链接: 图片全部下载在E:\taonvlang\目录下按淘女郎名称创建文件夹保存.
python爬虫流程-Python爬虫程序架构和运行流程原理解析
1 前言 Python开发网络爬虫获取网页数据的基本流程为: 发起请求通过URL向服务器发起request请求,请求可以包含额外的header信息. 获取响应内容服务器正常响应,将会收到一个res ...
2021年大数据Hadoop（二十七）：YARN运行流程
全网最详细的Hadoop文章系列,强烈建议收藏加关注! 后面更新文章都会列出历史文章目录,帮助大家回顾知识重点. 目录本系列历史文章前言 Yarn运行流程本系列历史文章 2021年大数据Hado ...
SSL/TLS算法流程解析
SSL/TLS 早已不是陌生的词汇,然而其原理及细则却不是太容易记住.本文将试图通过一些简单图示呈现其流程原理,希望读者有所收获. 一.相关版本 Version Source Description ...
驰骋工作流引擎Silverlight版本的流程解析执行器-预计在6月份发布
关键字: Silverlight 驰骋工作流引擎流程解析执行器关于silverlight 版本的流程解析执行器的问题编写人: ccflow 发布日期: 2013/5/20 1, 为什么要开发Si ...
Python爬虫：scrapy 的运行流程和各模块的作用
scrapy的运行流程爬虫 -> 起始URL封装Request -> 爬虫中间件 -> 引擎 -> 调度器(Scheduler): 缓存请求, 请求去重调度器 -> ...
python基础爬虫的框架以及详细的运行流程
网络爬虫是什么? 网络爬虫就是:请求网站并提取数据的自动化程序网络爬虫能做什么? 网络爬虫被广泛用于互联网搜索引擎或其他类似网站,可以自动采集所有其能够访问到的页面内容,以获取或更新这些网站的内容和 ...

QGC关于SetMode运行流程解析

QGC与飞控连接后初始化，初始飞行模式为手动模式

飞行模式切换

QGC关于SetMode运行流程解析相关推荐

最新文章

热门文章