QGC代码架构解析:QGC初始加载及状态机
在收到飞机发来的心跳包后,消息发送给MultiVehicleManager,在MultiVehicleManager中,检查组件ID是否是MAV_COMP_ID_AUTOPILOT1,以及vehicleType不是GCS等之后,会创建一个Vehicle对象,并进入初始化流程。
mavlink_message_t中已经包含了sysid、compid信息。而心跳包mavlink_heartbeat_t中则包含了vehicleType、firmwareType等信息。
需要注意的是,与飞控通信中,组件ID是固定的:
MAV_COMP_ID_AUTOPILOT1,即每个飞控的组件ID都是1。
飞机发现、创建及初始化流程如下图所示: 
主要初始流程入口在InitialConnectStateMachine中以状态机实现,且部分子流程也是以状态机实现(至多嵌套了三层状态机):
由于获取信息需要使用同步方式,在
InitialConnectStateMachine状态机中,使用回调方式处理应答Ack,在回调中进入下一个处理阶段。参见:1. 请求的实现,以及模拟同步请求。
static constexpr const StateMachine::StateFn _rgStates[] = {
_stateRequestAutopilotVersion,
_stateRequestStandardModes,
_stateRequestCompInfo,
_stateRequestParameters,
_stateRequestMission,
_stateRequestGeoFence,
_stateRequestRallyPoints,
_stateSignalInitialConnectComplete
};
1. 请求的实现,以及模拟同步请求
请求飞机信息,使用MAV_CMD_REQUEST_MESSAGE命令字,请求对应的消息ID(即子命令,比如请求飞机版本信息MAVLINK_MSG_ID_AUTOPILOT_VERSION),以及子命令的参数。另外,使用命令MAV_CMD_SET_MESSAGE_INTERVAL让飞机定期周期响应子命令消息。
飞机端收到MESSAGE消息之后,先返回一个响应Ack(Ack中包含msgid,以及响应码,比如MAV_RESULT_ACCEPTED)。QGC收到该消息,继续处理之前发送的请求,实现代码主要有两个函数入口:Vehicle::requestMessage,Vehicle::_handleCommandAck(mavlink_message_t& message),以及一个主要的数据成员QMap<int, QMap<int, RequestMessageInfo_t*>> _requestMessageInfoMap。
MAV_CMD_REQUEST_MESSAGE消息的文档:How to Request & Stream Messages。处理流程示例:
你的程序 飞控
| |
| 1. 发送 MAV_CMD_SET_MESSAGE_INTERVAL
|----------------------------------------→ (请求:以1Hz发送BATTERY_STATUS)
|
| | 处理请求
| 2. 接收 COMMAND_ACK
|←---------------------------------------- (确认已接受)
|
| 3. 等待 BATTERY_STATUS
|
| | 自动发送电池信息(周期: 1秒)
| ← 接收 BATTERY_STATUS #1 |
| ← 接收 BATTERY_STATUS #2 | (自动循环,无需请求)
| ← 接收 BATTERY_STATUS #3 |
| ← 接收 BATTERY_STATUS #4 |
2. 请求飞机版本信息(MAVLINK_MSG_ID_AUTOPILOT_VERSION)
主要获取飞机的编号、固件的vender_id、product_id,固件版本信息,以及capabilities(64位bitmask),capabilities相关枚举定义在MAVLink协议的MAV_PROTOCOL_CAPABILITY中。
3. 请求飞机标准模式(MAVLINK_MSG_ID_AVAILABLE_MODES)
主要获取飞机支持的标准模式。获取的模式列表用于设置给FirmwarePlugin,并在Vehicle中使用。
4. 请求组件元数据(META)信息
由于这一步请求处理多个类型META数据文件,整个处理放在单独的模块(源码文件)ComponentInformationManager中,且也使用状态机来实现:请求General元数据、Param元数据、Events元数据、Actuator元数据。General是指组件的信息(主要是飞控自身),而Events,Actuator不一定每个组件都有。
请求每个子分类的META数据,分为几个步骤(还是用状态机实现),在RequestMetaDataTypeStateMachine中实现:请求数据文件的地址URI(返回URI,以及CRC),根据URI请求META数据文件内容(具有缓存功能,先比较CRC,不相等再请求远程META文件),即数据类型描述文件。比如下一个步骤请求飞机的参数信息,就需要将请求到的参数生成Fact,而Fact的类型信息就来自于参数的META数据文件。
在请求
META数据文件的过程中,定义了两个数据类:1.struct CompInfo::Uris:存放META文件的URI,CRC,以及其他一些信息(如Fallback请求信息)。2.CompInfo,以及继承自CompInfo的上述几种META文件对应的子类:CompInfoGeneral,CompInfoParam,CompInfoEvents,CompInfoActuators。其中最重要也是首先需要请求的META文件是CompInfoGeneral,因为这个文件里面包含了设备支持的META文件类型列表(数据成员QMap<COMP_METADATA_TYPE, Uris> _supportedTypes;)。后续几个请求,要先检查设备是否支持该类型的META文件。
在
ComponentInformationManager中,维护了一个QMap<uint8_t /* compId */, QMap<COMP_METADATA_TYPE, CompInfo*>> _compInfoMap;,用于存放每个组件的各类META文件对象。这与上面所述的逻辑连接起来。
Events是MAVLink协议中的一个系统事件和诊断机制,用于飞机端向地面站实时报告系统事件、警告和错误。相关文档:Events Interface (WIP)。比如可能有如下事件:
飞机端事件流:
├─ 电池低电量事件
├─ GPS 信号丢失事件
├─ IMU 过热警告
├─ 传感器校准失败
├─ 电机故障检测
└─ 健康检查失败 (Health & Arming Checks)
4.1. META数据使用流程
请求到的META数据文件,主要用于创建FactMetaData对象,进而创建Fact对象。以请求参数的META数据文件为例,流程如下所示:
┌───────────────────────────────────────────────────┐
│ 飞机端 (Autopilot) │
└───────────────────────────────────────────────────┘
│
│ MAVLink
│
┌─────────────▼──────────────┐
│ ComponentInformation │
│ ┌──────────────────────┐ │
│ │ COMP_METADATA_TYPE │ │
│ │ _PARAMETER │ │
│ │ (JSON 文件 URI) │ │
│ └──────────────────────┘ │
└─────────────┬──────────────┘
│ 下载 JSON
│
┌─────────────▼──────────────┐
│ CompInfoParam.setJson() │
│ (解析 JSON 文件) │
└─────────────┬──────────────┘
│
┌─────────────▼──────────────────────────────┐
│ FactMetaData::createFromJsonObject() │
│ (将 JSON 转换为 FactMetaData 对象) │
└─────────────┬──────────────────────────────┘
│
┌─────────────▼──────────────────────────────┐
│ ParameterManager │
│ _nameToMetaDataMap[paramName] │
│ (存储所有参数的元数据) │
└────────────────────────────────────────────┘
代码执行流程:
// 1️⃣ 初始化连接时,请求参数的元数据
_stateRequestCompInfoEvents()
└─► _requestTypeStateMachine.request(
_compInfoMap[MAV_COMP_ID_AUTOPILOT1][COMP_METADATA_TYPE_PARAMETER]
);
// 2️⃣ 下载 JSON 文件后,调用 setJson()
CompInfoParam::setJson(const QString& metadataJsonFileName)
{
// 3️⃣ 解析 JSON 文件
QJsonDocument jsonDoc = // 从文件读取
QJsonArray rgParameters = jsonDoc["QGC_PARAMETERS"].toArray();
// 4️⃣ 为每个参数创建 FactMetaData 对象
for (QJsonValue parameterValue : rgParameters) {
FactMetaData* newMetaData =
FactMetaData::createFromJsonObject(parameterValue.toObject(), ...);
// 5️⃣ 存储到 map 中
_nameToMetaDataMap[newMetaData->name()] = newMetaData;
}
}
// 6️⃣ 后续使用时
FactMetaData* meta = _compInfoParam->factMetaDataForName("PARAM_NAME");
// 使用 meta 来验证、转换参数值
4.2. 对应的 MAVLink 服务
请求META数据使用微服务Component Metadata Protocol (WIP),命令字:MAVLINK_MSG_ID_COMPONENT_METADATA。针对各个META数据类型,提供了枚举定义COMP_METADATA_TYPE。
请求流程图如下所示:
sequenceDiagram;
participant Client
participant Server
Note over Server, Client: Client: Request component information.
Client->>Server: MAV_CMD_REQUEST_MESSAGE(param1=397)
Client-->>Client: Start ACK receive timeout
Server->>Client: CMD_ACK
Server->>Client: COMPONENT_METADATA( uri, file_crc)
Note over Server, Client: Client check file at uri has changed (using CRC in file_crc).
Note over Server, Client: Client download file at uri using MAVFTP and parse.
Note over Server, Client: Client download other metadata types referenced in general metadata<br> (from device or Internet).
5. 请求系统参数列表
这个步骤请求飞机的所有参数,使用MAVLink的微服务Parameter Protocol,在QGC的ParameterManager模块中实现,见上一篇QGC代码架构解析:MAVLink参数服务及QGC参数管理模块。
请求参数,依赖于上一步骤,即请求的参数META数据文件,用于创建参数对应的FactMetaData对象。
6. 请求任务列表(航点列表)
这个步骤使用Mission Protocol,直接调用PlanManager::loadFromVehicle下载飞机航点信息,进行初始化同步。具体参考下一篇QGC代码架构解析:MAVLink Mission Protocol,以及 QGC 航点管理。
由于MAVLink v2中,Mission Protocol不仅仅包含航点,还包含地理围栏(GeoFence)、降落点(Rally Points)等信息。所有这些部分都实现在PlanManager以及其继承子类中。
7. 请求地理围栏列表
参考6。
8. 请求降落点列表
参考6。
9. 初始化完成
完成初始化,发送signal,通知QML界面。
Enjoy Reading This Article?
Here are some more articles you might like to read next: