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