文章

QGC代码架构解析：MAVLink Mission Protocol，以及 QGC 航点管理

发表于 更新于
作者 Roderick.Huang
15 分钟阅读
QGC代码架构解析：MAVLink Mission Protocol，以及 QGC 航点管理

本文厘清如下几个问题：

  • 航点协议（Mission Protocol）有哪些命令/消息；
  • 微服务的处理流程，以及流程中通信双方使用到的命令ID/请求ID-响应ID；
  • 带参数的命令/消息，其参数格式，以及单位；
  • 航点文件格式。

航点协议主要实现航点集合的上传、下载、清除。另外有一些其他消息，以及附带的参数/枚举。

航点上传/下载使用到如下几个消息定义：

消息名称说明发送方
MISSION_REQUEST_LIST启动航点集下载动作地面站
MISSION_COUNT航点数量地面站/飞控
MISSION_REQUEST_INT请求航点地面站/飞控
MISSION_ITEM_INT请求航点飞控/地面站
MISSION_ACK航点响应飞控/地面站

1.1. 航点的上传/下载流程

航点上传/下载流程图（左边为QGC请求下载，右边为QGC请求上传）：

航点上传下载流程图

对比发现规律：

  • QGC请求下载时，使用MISSION_REQUEST_LIST来启动，而请求上传时，使用MISSION_COUNT来启动（注意：不论上传/下载，都是QGC发起的）；
  • MISSION_COUNT既可以作为飞控响应QGC的下载请求，也可以作为QGC请求飞控上传航点的启动消息；
  • 启动之后，中间航点的请求，双方使用MISSION_REQUEST_INT <–> MISSION_ITEM_INT配对，即一个完整的单个航点传输流程。航点发送方使用MISSION_REQUEST_INT请求，接收方使用MISSION_ITEM_INT响应。
  • 最后使用MISSION_ACK作为结束标示。在这里，ACK消息的含义是结束，即整个流程结束了，这样理解更合理。
  • 从文档看，MISSION_ACK只用在上传/下载流程里面。其他消息里面没有使用到。

上传/下载的流程，不具有对称性，给理解带来了一定的混乱。流程步骤在语义上理解也不顺畅。

1.1.1. 协议实现注意事项

  • 在流程图的Start timeout处，需要实现超时重传机制：
    • MISSION_REQUEST_LIST超时没有响应，重传该命令若干次；
    • MISSION_COUNT超时没有响应，重传该命令若干次；
    • MISSION_REQUEST_INT超时没有响应，重传该命令若干次；
  • MISSION_REQUEST_INT请求的航点，需要按序号顺序请求以及应答。如果收到的MISSION_ITEM_INT中的航点顺序不对，需要丢弃该航点数据，并重新请求。
  • 当上传/下载航点时，飞机会返回一个opaque_id（类似整个航点集合计算得到的哈希），用于避免不不必要的再次上传/下载。当QGC上传时，飞机在最后一个消息返回该值：MISSION_ACK.opaque_id。当QGC下载时，飞机在MISSION_COUNT中返回该值：MISSION_COUNT.opaque_id。
  • 当上传/下载的过程中失败时（对方提前返回MISSION_ACK并包含错误消息），QGC或者飞机应该终止当前流程，并恢复使用上一次的航点集。
  • 协议没有说明，上传过程中，最后一步，如果飞机没有返回MISSION_ACK，应该如何处理。QGC的实际处理方式是，认为上传成功并完成，但是opaque_id没有更新。

1.2. 航点消息结构：消息 MISSION_ITEM_INT，以及 MAV_CMD

航点不仅仅只有坐标等数据，还包含动作含义，即MAV_CMD其实是一个命令+参数数据。MISSION_ITEM_INT就是用于发送这些MAV_CMD子命令+参数的。MAV_CMD分为如下几类：

  • MAV_CMD_NAV_*：导航类命令（起飞、降落、返回RTL、悬停、飞到指定航点位置），比如MAV_CMD_NAV_WAYPOINT表示普通航点，MAV_CMD_NAV_LOITER_UNLIM表示无限悬停等。
  • MAV_CMD_DO_*：动作类命令，比如MAV_CMD_DO_CHANGE_SPEED表示改变速度，MAV_CMD_DO_SET_RELAY表示设置继电器等；
  • MAV_CMD_CONDITION_*：命令执行条件，比如MAV_CMD_CONDITION_DELAY表示等待一段时间之后，再执行下一个航点MAV_CMD。从Ardupilot文档看，MAV_CMD_CONDITION_*命令是作用于MAV_CMD_DO_*命令，参考Ardupilot – Mission Commands – Conditional commands。

主要的MAV_CMD_NAV命令列表：

Command IDNameDescription
16MAV_CMD_NAV_WAYPOINTNavigate to a specific waypoint
21MAV_CMD_NAV_LANDLand at the specified location
22MAV_CMD_NAV_TAKEOFFTake off and ascend to specified altitude
20MAV_CMD_NAV_RETURN_TO_LAUNCHReturn to launch/home location
80MAV_CMD_NAV_ROISets region of interest for camera
82MAV_CMD_NAV_SPLINE_WAYPOINTNavigate using a spline path

所有MAV_CMD命令的完整列表，以及参数，可以参考MAVLink协议文档：Commands (MAV_CMD)。

1.3. 航点命令的参数：MISSION_ITEM_INT的参数：Frame（坐标系）

在使用MISSION_ITEM_INT消息发送航点命令(Mission Item)时（包括MAV_CMD_NAV_*命令，以及MAV_CMD_DO_*命令），需要指定坐标系frame，比如WGS84坐标系，NED坐标系，或者在WGS84坐标系的修改，如高度改为相对HOME点高度，或者地形高度。坐标系枚举定义见：MAV_FRAME。

官方文档整理下来，有关Frame的描述，没有完全讲清楚，整个MAVLink协议中，有若干个命令使用到Frame作为参数。常见的需要使用到Frame的命令：

  • Mission Protocol中少量MAV_CMD_DO。
  • COMMAND_INT需要使用Frame作为命令参数：Command Protocol。

根据文档描述，针对MISSION_ITEM_INT，目前APM以及PX4仅支持global类型的Frame，见文档描述Mission Items (MAVLink Commands)。另外，有少量的MAV_CMD_DO命令，里面也带有frame参数，很奇怪，有些混乱。

更多信息：

1.4. 航点命令的参数：MISSION_ITEM_INT的参数：param1 ~ param7

MISSION_ITEM_INT_params

前四个参数param1 ~ param4，具体含义，以及单位，由具体的MAV_CMD命令决定，且数据类型就是float，即如果是其他类型，需要static_cast转换为float。

针对MAV_CMD_NAV命令，则是坐标信息，其中param5、param6（经纬度）是全局坐标，即1e7。高度参数param7，其含义由frame指定（但应该全部都是global类型的），global坐标系列表：

  • MAV_FRAME_GLOBAL_INT，
  • MAV_FRAME_GLOBAL_RELATIVE_ALT_INT，
  • MAV_FRAME_GLOBAL_TERRAIN_ALT，
  • MAV_FRAME_GLOBAL_TERRAIN_ALT_INT，
  • MAV_FRAME_GLOBAL。

另外，如果frame是MAV_FRAME_MISSION，表示这param5 ~ param7不是坐标，所以实现发送/接收的时候，不需要将param5 ~ param7乘以1e7。

根据协议，如果是frame的类型是local的，则发送/接收的时候param5、param6的精度应该是1e4，单位是米（参考协议文档Frames & Positional Information）。

总结：协议实现的时候：

  • 如果frame是MAV_FRAME_MISSION：则param5、param6按原样发送值，可能需要static_cast<int>转换。
  • 如果frame是global类型的：则param5、param6需要乘以1e7进行发送，接收时需要除以1e7。
  • 如果frame是local类型的：则param5、param6需要乘以1e4进行发送，接收时需要除以1e4。
  • 其他param1 ~ param4，以及param7，按原样发送/接收，可能需要static_cast<float>转换。

另外，这个规则，应该也适用于COMMAND_INT命令COMMAND_INT。且：

  • 使用COMMAND_INT命令时，有些参数没有使用到，部分样例参考：ardupilot – Commands supported by Copter
  • 这些浮点类型的参数，值nan也有有意义的，比如维持原值，具体搜索QGC代码中qQNaN()的使用地方。

1.5. 航点管理中其他命令/消息

如下两个消息，用来监控航点执行进度及状态：

  • MISSION_CURRENT：当前航点改变通知消息，由飞控广播，其他信息：序号seq，一起当前飞机航点状态，比如是否是暂停等。
  • MISSION_ITEM_REACHED：与MISSION_CURRENT类似，QGC中没有处理该消息。

清除航点使用消息：MISSION_CLEAR_ALL。

2. QGC 航点管理实现

PlanManager实现Mission Protocol的协议。由于MAVLink v2中将航点(flight plans)、地理围栏(geofences)、降落点(rally/safe points)都放在Mission Protocol中，在请求上传(MISSION_COUNT)/下载(MISSION_REQUEST_INT)，以及传输(MISSION_ITEM_INT)时，带有MAV_MISSION_TYPE字段，确定是哪种Mission Type，参考协议文档：Mission Protocol – Mission Types。

  • MissionManager：拓展航点的协议MAV_MISSION_TYPE_MISSION的固件实现：主要实现Ardupilot相关的实现，以及一些功能。
  • GeoFenceManager：实现MAV_MISSION_TYPE_FENCE。
  • RallyPointManager：实现MAV_MISSION_TYPE_RALLY。

2.1. 航点协议上传/下载功能的实现

PlanManager使用状态机实现上传/下载流程。定义一个TransactionType_t枚举，表示当前的事务类型，以及一个AckType_t表示期望的ACK类型：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

typedef enum {
    AckNone,            ///< State machine is idle
    AckMissionCount,    ///< MISSION_COUNT message expected
    AckMissionItem,     ///< MISSION_ITEM expected
    AckMissionRequest,  ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
    AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
    AckGuidedItem,      ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
} AckType_t;

typedef enum {
    TransactionNone,
    TransactionRead,
    TransactionWrite,
    TransactionRemoveAll
} TransactionType_t;

请求下载入口函数：PlanManager::loadFromVehicle，请求上传入口函数：PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems)。以及一个handle函数，用于处理收到的消息：

  • PlanManager::_handleMissionCount：处理MISSION_COUNT消息（请求下载）；
  • PlanManager::_handleMissionItem：处理MISSION_ITEM_INT消息（请求下载）；
  • PlanManager::_handleMissionRequestInt：处理MISSION_REQUEST_INT消息（请求上传）；

PlanManager中，有两个函数处理ACK消息：_handleMissionAck，_ackTimeout，这两个函数驱动状态机的流转：判断ACK与请求步骤是否匹配，发送一下一个航点数据/请求下一个航点数据，以及结束流程。

2.2. 其他模块

  • MissionItem：表示单个航点数据结构，航点管理模块的基础数据类。
  • MissionController：提供QML接口访问航点数据。
  • PlanMasterController：航点管理模块的顶层控制类（入口），提供QML接口访问航点（MissionController）、电子围栏（GeoFenceController）、降落点（RallyPointController）。以及从文件中加载/保存（包含kml文件格式）。
  • MissionCommandTree：提供MAV_CMD命令树结构，供QML界面使用。

2.3. 航点文件格式

QGC中，航点文件格式使用JSON格式，文件扩展名为.plan，保存目录样例：

1

C:\Users\Administrator\Documents\QGroundControl Daily\Missions

保存时，将航点数据、电子围栏数据、降落点数据，保存到同一个文件中。保存及加载函数入口：

1
2
3
4
5

PlanMasterController::saveToJson();
MissionController::save(QJsonObject& json);

// 加载实现
bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);

另外还可以导出/导入kml格式的航点文件，记录的字段格式有所不同，另外测试发现加载有些BUG。

航点有SimpleItem，以及ComplexItem，一个简单的航点文件样例：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

{
    "fileType": "Plan",
    "version": 1,
    "groundStation": "QGroundControl",
    "mission": {
        "cruiseSpeed": 5,
        "hoverSpeed": 3,
        "items": [
            {
                "AMSLAltAboveTerrain": null,
                "autoContinue": true,
                "command": 16,
                "frame": 3,
                "params": [0, 0, 0, null, 47.397742, 8.545594, 488],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": null,
                "autoContinue": true,
                "command": 16,
                "frame": 3,
                "params": [0, 0, 0, null, 47.397825, 8.545632, 488],
                "type": "SimpleItem"
            }
        ]
    }
}

有关航点文件格式的更多信息，参考QGC代码仓库文档：

3. 参考文档

QGC
QGC
本文由作者按照 CC BY 4.0 进行授权