QGC 航点协议分析总结

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);

}

  • 3
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值