QGC关于SetMode运行流程解析

小驴淘米666

已于 2022-11-01 10:27:45 修改

阅读量1.5k

点赞数 1

分类专栏： QGC 文章标签：开发语言

于 2022-10-31 17:29:55 首次发布

本文链接：https://blog.csdn.net/nieji3057/article/details/127617132

版权

QGC 专栏收录该内容

8 篇文章 8 订阅

订阅专栏

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.verticalCenter
    alternateText:          _activeVehicle ? _activeVehicle.flightMode : ""
    model:                  _flightModes
    font.pointSize:         ScreenTools.mediumFontPointSize
    currentIndex:           -1
    sizeToContents:         true

    property bool showIndicator: true

    property var _activeVehicle:    QGroundControl.multiVehicleManager.activeVehicle
    property 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