diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index e177e215bfaf7355c3bea2fd4aba3ce79254e6ec..9f85c02980684c63e778b19e1d80a86a8915f954 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -24,8 +24,7 @@ MissionManager::MissionManager(Vehicle* vehicle) , _dedicatedLink(NULL) , _ackTimeoutTimer(NULL) , _expectedAck(AckNone) - , _readTransactionInProgress(false) - , _writeTransactionInProgress(false) + , _transactionInProgress(TransactionNone) , _resumeMission(false) , _lastMissionRequest(-1) , _currentMissionIndex(-1) @@ -58,7 +57,7 @@ void MissionManager::_writeMissionItemsWorker(void) _itemIndicesToWrite << i; } - _writeTransactionInProgress = true; + _transactionInProgress = TransactionWrite; _retryCount = 0; emit inProgressChanged(true); _writeMissionCount(); @@ -136,7 +135,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC return; } - _writeTransactionInProgress = true; + _transactionInProgress = TransactionWrite; mavlink_message_t messageOut; mavlink_mission_item_t missionItem; @@ -183,7 +182,7 @@ void MissionManager::requestMissionItems(void) } _retryCount = 0; - _readTransactionInProgress = true; + _transactionInProgress = TransactionRead; emit inProgressChanged(true); _requestList(); } @@ -271,6 +270,17 @@ void MissionManager::_ackTimeout(void) _finishTransaction(false); } break; + case AckMissionClearAll: + // MISSION_ACK expected + if (_retryCount > _maxRetryCount) { + _sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded.")); + _finishTransaction(false); + } else { + _retryCount++; + qCDebug(MissionManagerLog) << "Retrying MISSION_CLEAR_ALL retry Count" << _retryCount; + _removeAllWorker(); + } + break; case AckGuidedItem: // MISSION_REQUEST is expected, or MISSION_ACK to end sequence default: @@ -514,16 +524,17 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo } mavlink_msg_mission_request_decode(&message, &missionRequest); + qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq; + + if (missionRequest.seq > _missionItems.count() - 1) { + _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq)); + _finishTransaction(false); + return; + } _lastMissionRequest = missionRequest.seq; if (!_itemIndicesToWrite.contains(missionRequest.seq)) { - if (missionRequest.seq > _missionItems.count()) { - _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_missionItems.count()).arg(missionRequest.seq)); - _finishTransaction(false); - return; - } else { - qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq; - } + qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq; } else { _itemIndicesToWrite.removeOne(missionRequest.seq); } @@ -635,6 +646,13 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) _finishTransaction(false); } break; + case AckMissionClearAll: + // MISSION_ACK expected + if (missionAck.type != MAV_MISSION_ACCEPTED) { + _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + } + _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED); + break; case AckGuidedItem: // MISSION_REQUEST is expected, or MISSION_ACK to end sequence if (missionAck.type == MAV_MISSION_ACCEPTED) { @@ -807,7 +825,7 @@ QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result) void MissionManager::_finishTransaction(bool success) { - if (!success && _readTransactionInProgress) { + if (!success && _transactionInProgress == TransactionRead) { // Read from vehicle failed, clear partial list _clearAndDeleteMissionItems(); emit newMissionItemsAvailable(false); @@ -816,9 +834,8 @@ void MissionManager::_finishTransaction(bool success) _itemIndicesToRead.clear(); _itemIndicesToWrite.clear(); - if (_readTransactionInProgress || _writeTransactionInProgress) { - _readTransactionInProgress = false; - _writeTransactionInProgress = false; + if (_transactionInProgress != TransactionNone) { + _transactionInProgress = TransactionNone; emit inProgressChanged(false); } @@ -830,7 +847,7 @@ void MissionManager::_finishTransaction(bool success) bool MissionManager::inProgress(void) { - return _readTransactionInProgress || _writeTransactionInProgress; + return _transactionInProgress != TransactionNone; } void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) @@ -852,11 +869,45 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message) } } +void MissionManager::_removeAllWorker(void) +{ + mavlink_message_t message; + + qCDebug(MissionManagerLog) << "_removeAllWorker"; + + _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + _vehicle->id(), + MAV_COMP_ID_MISSIONPLANNER, + MAV_MISSION_TYPE_MISSION); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); + _startAckTimeout(AckMissionClearAll); +} + void MissionManager::removeAll(void) { - QList emptyList; + if (inProgress()) { + return; + } + + qCDebug(MissionManagerLog) << "removeAll"; + + _clearAndDeleteMissionItems(); + + _currentMissionIndex = -1; + _lastCurrentIndex = -1; + emit currentIndexChanged(-1); + emit lastCurrentIndexChanged(-1); + emit newMissionItemsAvailable(true /* removeAllRequested */); + + _transactionInProgress = TransactionClearAll; + _retryCount = 0; + emit inProgressChanged(true); - writeMissionItems(emptyList); + _removeAllWorker(); } void MissionManager::generateResumeMission(int resumeIndex) diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 7a0f919d63cb3dd19cf05467522b23f7c76d184a..9a235cfd6b1f9712f16957a877c5f7e0101028b9 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -95,9 +95,17 @@ private: 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, + TransactionClearAll + } TransactionType_t; + void _startAckTimeout(AckType_t ack); bool _checkForExpectedAck(AckType_t receivedAck); void _readTransactionComplete(void); @@ -117,6 +125,7 @@ private: void _writeMissionItemsWorker(void); void _clearAndDeleteMissionItems(void); QString _lastMissionReqestString(MAV_MISSION_RESULT result); + void _removeAllWorker(void); private: Vehicle* _vehicle; @@ -126,12 +135,11 @@ private: AckType_t _expectedAck; int _retryCount; - bool _readTransactionInProgress; - bool _writeTransactionInProgress; - bool _resumeMission; - QList _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle - QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle - int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST + TransactionType_t _transactionInProgress; + bool _resumeMission; + QList _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle + QList _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle + int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST QMutex _dataMutex;