QGC 航点协议分析总结
- 上传航点过程
1 .1点击 waypoint
1.2 在地图上添加航点
1.2.1 MouseArea中
// 鼠标当前点 转为十进制 经纬高
MouseArea { anchors.fill: parent onClicked: { // Take focus to close any previous editing editorMap.focus = true var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) ....................... } |
1.2.2 调用insertSimpleItemAfterCurrent(coordinate)
function insertSimpleItemAfterCurrent(coordinate) { var nextIndex = _missionController.currentPlanViewVIIndex + 1 _missionController.insertSimpleMissionItem(coordinate,nextIndex,true/* makeCurrentItem */) } |
1.2.3 insertSimpleMissionItem
首先MissionController.h中对该函数的声明;
/// Add a new simple mission item to the list 添加一个新的简单任务项到列表 ///@param coordinate: Coordinate for item item的坐标 ///@param visualItemIndex: index to insert at, -1 for end of list 要插入的索引,-1表示列表的结束 ///@param makeCurrentItem: true: Make this item the current item 使该项为当前项 ///@return Newly created item 新创建的项 Q_INVOKABLE VisualMissionItem* insertSimpleMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem = false); |
MissionController.cc的实现函数:
VisualMissionItem* MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem) { return _insertSimpleMissionItemWorker(coordinate, MAV_CMD_NAV_WAYPOINT, visualItemIndex, makeCurrentItem); } |
注:MAV_CMD_NAV_WAYPOINT (导航到坐标,waypoint坐标参数等) 格式如下
Param (:Label) | Description |
1: Hold | 保存时间。(固定翼忽略,旋转翼停留在航路点的时间) |
2: Accept Radius | 接受半径(如果有此半径的球体被击中,路径点计数为到达) |
3: Pass Radius | 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. |
4: Yaw | 航路点的期望偏航角(旋翼)。NaN使用当前系统的偏航航向模式(例如偏航至下一个航路点,偏航至home等)。 |
5: Latitude | 纬度 |
6: Longitude | 经度 |
7: Altitude | 高度 |
1.2.4 _insertSimpleMissionItemWorker()
返回的 _insertSimpleMissionItemWorker() 函数 生成一个newItem对象 通过对对象set参数,最后返回该对象
VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); SimpleMissionItem * newItem = new SimpleMissionItem(_masterController, _flyView, false /* forLoad */, this); newItem->setSequenceNumber(sequenceNumber);// set 序号 newItem->setCoordinate(coordinate);//set 坐标 newItem->setCommand(command);//set command _initVisualItem(newItem); // set高度 if (newItem->specifiesAltitude()) { if (!qgcApp()->toolbox()->missionCommandTree()->isLandCommand(command)) { double prevAltitude; int prevAltitudeMode; if (_findPreviousAltitude(visualItemIndex, &prevAltitude, &prevAltitudeMode)) { newItem->altitude()->setRawValue(prevAltitude); if (globalAltitudeMode() == QGroundControlQmlGlobal::AltitudeModeNone) { newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode)); } } }} if (visualItemIndex == -1) { _visualItems->append(newItem);} else { _visualItems->insert(visualItemIndex, newItem);} _recalcAllWithCoordinate(coordinate); if (makeCurrentItem) { setCurrentPlanViewSeqNum(newItem->sequenceNumber(), true); } _firstItemAdded(); return newItem; } |
- 上传Upload
上传upload的逻辑:上传 Upload
PlanView.qml upload()->sendToVehicle()->MissionController.h sendToVehicle()
->sendItemsToVehicle(_managerVehicle,_visualItems)
-> vehicle missionManager()->writeMissionItems(rgMissionItems)
->_writeMissionItemsWorker()->MissionManager.cc_writeMissionItemsWorker-> _writeMissionCount()
mavlink 写入总个数,并发送回应请求_startAckTimeout(AckMissionRequest);
->_handleMissionRequest( ) mavlink收到请求,开始通过 航点序号发送航点
总结:先发送总个数,收到回复后,开始发送航点。按照 回复-》发送->回复-》发送的方式。
2.1 PlanView.qml
2.1.1 Upload按钮
按下按钮后调用 _planMasterController.upload()
QGCButton { text: qsTr("Upload") Layout.fillWidth: true enabled: !_planMasterController.offline && !_planMasterController.syncInProgress && _planMasterController.containsItems visible: !QGroundControl.corePlugin.options.disableVehicleConnection onClicked: { dropPanel.hide() _planMasterController.upload() } } |
2.1.2 upload()
判断后 调用sendToVehicle().
function upload() { if (!checkReadyForSaveUpload(false /* save */)) { return } switch (_missionController.sendToVehiclePreCheck()) { // ok的 case MissionController.SendToVehiclePreCheckStateOk: sendToVehicle() break //在上传新的计划之前,必须暂停当前的任务 case MissionController.SendToVehiclePreCheckStateActiveMission: mainWindow.showMessageDialog(qsTr("Send To Vehicle"), qsTr("Current mission must be paused prior to uploading a new Plan")) break case MissionController.SendToVehiclePreCheckStateFirwmareVehicleMismatch: mainWindow.showComponentDialog(firmwareOrVehicleMismatchUploadDialogComponent, qsTr("Plan Upload"), mainWindow.showDialogDefaultWidth, StandardButton.Ok | StandardButton.Cancel) break } } |
2.2 MissionController.cc
2.2.1 sendToVehicle()
MissionController.cc中 通过判断后 调用 sendItemsToVehicle(_managerVehicle, _visualItems);
void MissionController::sendToVehicle(void) { if (_masterController->offline()) { qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while offline"; } else if (syncInProgress()) { qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress"; } else { qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle"; if (_visualItems->count() == 1) { //这可以防止我们向车辆发送一个可能伪造的位置 QmlObjectListModel emptyModel; sendItemsToVehicle(_managerVehicle, &emptyModel); } else { sendItemsToVehicle(_managerVehicle, _visualItems); } setDirty(false);}} |
2.2.2 sendItemsToVehicle()
传入参数 (设备和任务)、先为任务加入参数,然后调用PlanManager.cc的writeMissionItem
void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems) { if (vehicle) { QList<MissionItem*> rgMissionItems; _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle); vehicle->missionManager()->writeMissionItems(rgMissionItems); }} |
注: _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);该函数为为任务加入内容
_convertToMissionItems(visualMissionItems, rgMissionItems, vehicle)
bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent) { if (visualMissionItems->count() == 0) { return false; } bool endActionSet = false; int lastSeqNum = 0; for (int i=0; i<visualMissionItems->count(); i++) { VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i)); lastSeqNum = visualItem->lastSequenceNumber(); visualItem->appendMissionItems(rgMissionItems, missionItemParent); qCDebug(MissionControllerLog) << "_convertToMissionItems seqNum:lastSeqNum:command" << visualItem->sequenceNumber() << lastSeqNum << visualItem->commandName(); } // Mission settings has a special case for end mission action MissionSettingsItem* settingsItem = visualMissionItems->value<MissionSettingsItem*>(0); if (settingsItem) { endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent); } return endActionSet; } |
2.3 PlanMananger.cc
调用此类的写入任务函数
2.3.1 writeMissionItems
void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems) { if (_vehicle->isOfflineEditingVehicle()) { return; } if (inProgress()) { qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 called while transaction in progress").arg(_planTypeString()); return; } _clearAndDeleteWriteMissionItems(); bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); int firstIndex = skipFirstItem ? 1 : 0; for (int i=firstIndex; i<missionItems.count(); i++) { MissionItem* item = missionItems[i]; _writeMissionItems.append(item); // PlanManager接管已通过的MissionItem item->setIsCurrentItem(i == firstIndex); if (skipFirstItem) { item->setSequenceNumber(item->sequenceNumber() - 1); if (item->command() == MAV_CMD_DO_JUMP) { item->setParam1((int)item->param1() - 1); } } } _writeMissionItemsWorker();} |
判断与设备连接在进程中,判断规划类型,没咋看懂。然后调用_writeMissionItemWorker
2.3.2 _writeMissionItemsWorker()
void PlanManager::_writeMissionItemsWorker(void) { _lastMissionRequest = -1; emit progressPct(0); qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 count:").arg(_planTypeString()) << _writeMissionItems.count(); // Prime write list _itemIndicesToWrite.clear(); for (int i=0; i<_writeMissionItems.count(); i++) { _itemIndicesToWrite << i; } _retryCount = 0; _setTransactionInProgress(TransactionWrite); _connectToMavlink(); _writeMissionCount(); } |
调用_writeMissionCount(); 先写航点数
注释: _connectToMavlink();连接协议
void PlanManager::_connectToMavlink(void) { connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived); } |
通过查看_mavlinkMessageReceived 理解为飞控需要发送的mavlink协议
///当收到新的mavlink消息时调用 void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message) { switch (message.msgid) { case MAVLINK_MSG_ID_MISSION_COUNT: _handleMissionCount(message); break; case MAVLINK_MSG_ID_MISSION_ITEM: _handleMissionItem(message, false /* missionItemInt */); break; case MAVLINK_MSG_ID_MISSION_ITEM_INT: _handleMissionItem(message, true /* missionItemInt */); break; case MAVLINK_MSG_ID_MISSION_REQUEST: _handleMissionRequest(message, false /* missionItemInt */); break; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: _handleMissionRequest(message, true /* missionItemInt */); break; case MAVLINK_MSG_ID_MISSION_ACK: _handleMissionAck(message); break; } } |
2.3.3 _writeMissionCount();
void PlanManager::_writeMissionCount(void) { qCDebug(PlanManagerLog) << QStringLiteral("_writeMissionCount %1 count:_retryCount").arg(_planTypeString()) << _writeMissionItems.count() << _retryCount; WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); if (!weakLink.expired()) { mavlink_message_t message; SharedLinkInterfacePtr sharedLink = weakLink.lock(); mavlink_msg_mission_count_pack_chan( qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), sharedLink->mavlinkChannel(), &message, _vehicle->id(), MAV_COMP_ID_AUTOPILOT1, _writeMissionItems.count(), _planType); _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); } _startAckTimeout(AckMissionRequest); } |
Mavlink 写入总个数,并发送回应请求_startAckTimeout(AckMissionRequest)
2.3.4 _startAckTimeout(AckMissionRequest)
void PlanManager::_startAckTimeout(AckType_t ack) { switch (ack) { case AckMissionItem: _ackTimeoutTimer->setInterval(_retryTimeoutMilliseconds); break; case AckNone: case AckMissionCount: case AckMissionRequest: case AckMissionClearAll: case AckGuidedItem: _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds); break;} _expectedAck = ack; _ackTimeoutTimer->start(); } |
2.3.5 _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt) { MAV_MISSION_TYPE missionRequestMissionType; uint16_t missionRequestSeq; if (missionItemInt) { mavlink_mission_request_int_t missionRequest; mavlink_msg_mission_request_int_decode(&message, &missionRequest); missionRequestMissionType = static_cast<MAV_MISSION_TYPE>(missionRequest.mission_type); missionRequestSeq = missionRequest.seq; } else { mavlink_mission_request_t missionRequest; mavlink_msg_mission_request_decode(&message, &missionRequest); missionRequestMissionType = static_cast<MAV_MISSION_TYPE>(missionRequest.mission_type); missionRequestSeq = missionRequest.seq; } if (missionRequestMissionType != _planType) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequestMissionType; return; } if (!_checkForExpectedAck(AckMissionRequest)) { return; } qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequestSeq; if (missionRequestSeq > _writeMissionItems.count() - 1) { _sendError(RequestRangeError, tr("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequestSeq)); _finishTransaction(false); return; } emit progressPct((double)missionRequestSeq / (double)_writeMissionItems.count()); _lastMissionRequest = missionRequestSeq; if (!_itemIndicesToWrite.contains(missionRequestSeq)) { qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequestSeq; } else { _itemIndicesToWrite.removeOne(missionRequestSeq); } MissionItem* item = _writeMissionItems[missionRequestSeq]; qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequestSeq << item->command(); WeakLinkInterfacePtr weakLink = _vehicle->vehicleLinkManager()->primaryLink(); if (!weakLink.expired()) { bool forceMissionItemInt = (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_INT) && _vehicle->apmFirmware(); // ArduPilot always expects to get MISSION_ITEM_INT if possible mavlink_message_t messageOut; SharedLinkInterfacePtr sharedLink = weakLink.lock(); if (missionItemInt || forceMissionItemInt) { mavlink_msg_mission_item_int_pack_chan( qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), sharedLink->mavlinkChannel(), &messageOut, _vehicle->id(), MAV_COMP_ID_AUTOPILOT1, missionRequestSeq, item->frame(), item->command(), missionRequestSeq == 0, item->autoContinue(), item->param1(), item->param2(), item->param3(), item->param4(), item->frame() == MAV_FRAME_MISSION ? item->param5() : item->param5() * 1e7, item->frame() == MAV_FRAME_MISSION ? item->param6() : item->param6() * 1e7, item->param7(), _planType); } else { mavlink_msg_mission_item_pack_chan( qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), sharedLink->mavlinkChannel(), &messageOut, _vehicle->id(), MAV_COMP_ID_AUTOPILOT1, missionRequestSeq, item->frame(), item->command(), missionRequestSeq == 0, item->autoContinue(), item->param1(), item->param2(), item->param3(), item->param4(), item->param5(), item->param6(), item->param7(), _planType); } _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), messageOut); } _startAckTimeout(AckMissionRequest); } |