QGC地面站二次开发（三）Qt 简洁地面站

最新推荐文章于 2024-04-28 21:51:47 发布

Chris_Brown

最新推荐文章于 2024-04-28 21:51:47 发布

阅读量1.5w

点赞数 1

文章标签：嵌入式物联网 qt c++ ui

本文链接：https://blog.csdn.net/Neil_Chris/article/details/121436866

版权

多机控制原理

多机地面站支持 TCP、 UDP 和串口等三种连接方式，首先我们需要对这两种连接方式抽象出一个基类 LinkInterface，提供两个虚函数分别对应于数据接受和发送处理，针对不同的连接方式实现 TCP、 UDP 和串口连接相对应的子类TCPLink、 UDPLink 和 SerialLink 并在该类中完成将输入写入 socket 和串口的操作。再实现一个 MAVLinkProtocol 类，它实现将所有接受到的数据统一汇总到MAVlink 的消息解析的接口中，实现消息的解包，每个飞机对应于唯一的system id，当出现不同的 system id 时，构造一个飞机类。该飞机 Vehicle 类实现对飞机的控制(如解锁、起飞、航点等操作)以及数据接受显示(如飞行轨迹绘制等等)。这些多机通过 MultiVehicleManager 来多机的管理，其中多机管理中通过 activeVehicle 接口来选中当前活跃的飞机，进而实现对该飞机的单机控制。同样的，我们可以可以遍历连接的所有的飞机进而实现对所有飞机的控制

连接到多机构造的原理图

多机控制原理

多机控制实现

多机控制首先需要实现的是建立地面站和飞机之间的连接，将 TCP、 UDP 和串口通信抽象出一个LinkInterface 基类，实现父类 TCPLink、 UDPLink 和SerialLink 分别对应于 TCP、 UDP、串口的数读入和写入操作。

接口	void _writeBytes(const QByteArray)
意义	将数据写入 TCP、 UDP 和串口等。
输入	const QByteArray	写入的字节流
输出	无

接口

void bytesReceived(LinkInterface* link, QByteArray data);

意义

将接受到的字节流通过信号发射出去

输入

LinkInterface* link	接受到数据的链接
QByteArray data	接受到的字节流

输出

无

将接受到的所有数据汇聚到 MAVLinkProtocol 的 receiveBytes 接口中进行通过MAVLink 的解析接口进行消息的解析。

接口

void receiveBytes(LinkInterface* link, QByteArray b)

意义

将地面站接受到的字节解析为 MAVlink 消息包，并通过信号
messageReceived(LinkInterface* link, mavlink_message_t
message)将解析出的 MAVLink 结构体发射出去。

输入

LinkInterface* link	接受到数据的链接
QByteArray data	接受到的字节流

输出

无

同时再 MAVLinkProtocol 解析到心跳包还会发射 vehicleHeartbeatInfo 信号，在MultiVehicleManager 类的的_vehicleHeartbeatInfo 中进行处理，判断心跳中出现新的 system id 时构造一个 Vehicle 类。

当前选中的飞机
所在类	MultiVehicleManager
接口	Vehicle* activeVehicle()
属性	activeVehicle
WRITE	setActiveVehicle
NOTIFY	activeVehicleChanged

多机链表
所在类	MultiVehicleManager
接口	QmlObjectListModel* vehicles(void)
属性	vehicles

多机实现关系图

简洁地面站优化

1. 飞机的飞行轨迹以不同的颜色区分

主要的思想时预选一系列的颜色，将飞机的 ID 和轨迹颜色的相应序列号建立一一的对应关系，并在 Vehicle 中提供一个轨迹颜色的属性提供给 qml 进行访问。首先再 Vehicle 中添加三个静态变量和轨迹颜色属性：

public：
//连接的飞机的数，每多连接一台飞机该值加 1 ，即在
Vehicle::Vehicle(LinkInterface* link,……)构造函数中加一
static quint16 connectedVehicleCount;
//提供一组轨迹颜色
static QList<QColor> trajectoryColors;
//建立飞机 ID 和轨迹颜色序列号的对应
static QMap<int, int> vehicleIdAndConnectedSeq;
Q_PROPERTY(QColor trajectoryColor READ trajectoryColor
CONSTANT)

对 connectedVehicleCount 和 trajectoryColors 进行初始化

quint16 Vehicle::connectedVehicleCount = 0;
//http://www.sioe.cn/yingyong/yanse-rgb-16/
QList<QColor> Vehicle::trajectoryColors = {
QColor(0x00, 0x00, 0x00), //black unused
QColor(0xFF, 0xFF, 0x00), //Yellow
QColor(0xFF, 0x00, 0x00), //red
QColor(0xFF, 0x8C, 0x00), //DarkOrange
QColor(0xFF, 0x45, 0x00), //OrangeRed
QColor(0xD2, 0x69, 0x1E), //Chocolate
QColor(0xFA, 0x80, 0x72), //Salmon
QColor(0x8B, 0x00, 0x00), //DarkRed
QColor(0xDA, 0xA5, 0x20), //GoldEnrod
QColor(0xFF, 0x00, 0xFF), //Magenta
QColor(0x94, 0x00, 0xD3), //DarkVoilet
};
QMap<int, int> Vehicle::vehicleIdAndConnectedSeq;

在构造函数中添加对颜色的处理

//连接飞机数量加 1
Vehicle::connectedVehicleCount++;
//如果存储飞机 ID 和轨迹颜色序列号的对应的列表为空，则直接添加映射关系，
如果不为空的情况下需要考虑断开连接之后轨迹颜色资源的回收问题，可以通过遍历存
储的 ID 和颜色序列号映射表，直接该轨迹颜色；而如果没有直接在尾部追加即可。
if(Vehicle::vehicleIdAndConnectedSeq.isEmpty()){
Vehicle::vehicleIdAndConnectedSeq.insert(_id,
Vehicle::connectedVehicleCount);
}else{
QMap<int, int>::iterator tmp;
int querySeq = 1;
bool found = false;
for(tmp=Vehicle::vehicleIdAndConnectedSeq.begin();tmp !=
Vehicle::vehicleIdAndConnectedSeq.end();tmp++){
if(tmp.value() != querySeq){
found = true;
Vehicle::vehicleIdAndConnectedSeq.insert(_id, querySeq);
break;
}
querySeq++;
}
if(!found)
Vehicle::vehicleIdAndConnectedSeq.insert(_id,
Vehicle::connectedVehicleCount);
}

同时，断开连接之后，析构函数中将连接的飞机数减 1，同时删除飞机编号和飞机轨迹颜色的映射关系的映射辨析。

Vehicle::connectedVehicleCount--;
Vehicle::vehicleIdAndConnectedSeq.take(_id);

之后给 trajectoryColor 添加相关实现：

QColor Vehicle::trajectoryColor()
{
if(Vehicle::connectedVehicleCount < Vehicle::trajectoryColors.count()){
return
Vehicle::trajectoryColors.at(Vehicle::vehicleIdAndConnectedSeq.find(_id).val
ue());
}else{
//generate a color you can make it better
return QColor(0xFF,Vehicle::connectedVehicleCount * 0xF,
Vehicle::connectedVehicleCount * 8);
} }

之后在 FlightDisplayViewMap.qml 中绘制轨迹颜色取 Vehicle 中 trajectoryColor 即可。

// Add trajectory points to the map
MapItemView {
model: _mainIsMap ? _activeVehicle ? _activeVehicle.trajectoryPoints :
0 : 0
delegate: MapPolyline {
line.width: 3
line.color: _activeVehicle?_activeVehicle.trajectoryColor:"red"
z: QGroundControl.zOrderTrajectoryLines
path: [
object.coordinate1,
object.coordinate2,
]
}
}

2. 控制所有的飞机

GuidedActionsController 中通过 actionID 进行不同状态切换和指令的实现，实现控制所有的飞机解锁、上锁、返航、起飞、降落、紧急停机等，首先新建如下的 id，遍历 MultiVehicleManager 中的 Vehicles 调用相关的控制接口可以实现相关的操作。

readonly property int actionMVArm: 22
readonly property int actionMVDisarm: 23
readonly property int actionMVRTL: 24
readonly property int actionMVTakeoff: 25
readonly property int actionMVLand: 26
readonly property int actionMVEmergencyStop: 27

之后在 confirmAction(actionCode, actionData)中追加一系列消息提醒的。

case actionMVArm:
confirmDialog.title = qsTr("All arm")
confirmDialog.message = qsTr("Command all vehicles to arm")
confirmDialog.hideTrigger = true;
break;
case actionMVDisarm:
confirmDialog.title = qsTr("All disarm")
confirmDialog.message = qsTr("Command all vehicles to disarm")
confirmDialog.hideTrigger = true;
break;
case actionMVTakeoff:
confirmDialog.title = qsTr("All takeoff")
confirmDialog.message = qsTr("Command all vehicles to takeoff")
confirmDialog.hideTrigger = true;
break;
case actionMVRTL:
confirmDialog.title = qsTr("All RTL")
confirmDialog.message = qsTr("Command all vehicles to RTL")
confirmDialog.hideTrigger = true;
break;
case actionMVLand:
confirmDialog.title = qsTr("All Land")
confirmDialog.message = qsTr("Command all vehicles to Land")
confirmDialog.hideTrigger = true;
break;
case actionMVEmergencyStop:
confirmDialog.title = qsTr("All Emergency Stop")
confirmDialog.message = qsTr("Command all vehicles to Emergency
Stop")
confirmDialog.hideTrigger = true;
break;

executeAction 中追加代码实现当我们触发了确认操作需要执行的一系列动作。

case actionMVArm:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).armed = true
}
break
case actionMVDisarm:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).armed = false
}
break
case actionMVLand:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).guidedModeLand()
}
break
case actionMVRTL:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).guidedModeRTL()
}
break
case actionMVTakeoff:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).guidedModeTakeoff(actionAltitudeChange)
}
break;
case actionMVEmergencyStop:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
for (i = 0; i < rgVehicle.count; i++) {
rgVehicle.get(i).emergencyStop()
}
break

接下来我们在页面上添加出发动作的按钮，在 MultiVehicleList.qml 中添加：

Column {
id: mvCommandsColumn
anchors.margins: _margin
anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
text: qsTr("The following commands will be applied
to all vehicles")
color: _textColor
wrapMode: Text.WordWrap
font.pointSize: ScreenTools.smallFontPointSize
}
Row{
spacing: _margin
QGCButton {
text: qsTr("arm")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVArm)
}
QGCButton {
text: qsTr("disarm")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVDisarm
)
}
QGCButton {
text: qsTr("EmergencyStop")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVEmerge
ncyStop)
}
}
Row{
spacing: _margin
QGCButton {
text: qsTr("Takeoff")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVTakeof
f)
}
QGCButton {
text: qsTr("Land")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVLand)
}
QGCButton {
text: qsTr("RTL")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVRTL)
}
}
Row {
spacing: _margin
QGCButton {
text: qsTr("Pause")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVPause)
}
QGCButton {
text: qsTr("Start Mission")
onClicked:
guidedActionsController.confirmAction(guidedActionsController.actionMVStartM
ission)
}
}
}
}

在 FlightDisplayView.qml 给 MultiVehicleList 传 guidedActionsController属性值，实现鼠标点击触发操作动作。

MultiVehicleList {
anchors.margins: _margins
anchors.top: singleMultiSelector.bottom
anchors.right: parent.right
anchors.bottom: parent.bottom
width: ScreenTools.defaultFontPixelWidth * 30
visible: !singleVehicleView.checked
&& !QGroundControl.videoManager.fullScreen
z: _panel.z + 4
guidedActionsController:guidedActionsController
}

3. 将设定航线送给特定的飞机

该实现的主要的思想是当前活跃的飞机切换时，擦出之前规划的航线，同时后续发送航线时，将当前的航线信息通过当前活跃的飞机发射出去。PlanMasterController 的 start 函数将MultiVehicleManager 的activeVehicleChanged 信号和它自身的_activeVehicleChanged 槽函数绑定，来实现对当当前飞机更改时的相关操作。

void PlanMasterController::_activeVehicleChanged(Vehicle* activeVehicle)
{
if (_managerVehicle == activeVehicle) {
// We are already setup for this vehicle
return;
}
if (_managerVehicle) {
// Disconnect old vehicle
………
//删除地面站上的航点信息，未删除飞机中的航点信息
_missionController.removeAll();
_missionController.setDirty(false);
}