diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 1c7eae0aac09e17dd3eca5bc8f5a605e286412b5..ef35e2d43c37298229d000173ede2e2a9e8d5e93 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -90,7 +90,7 @@ void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlOb _sendFencePoint(index); } - emit sendComplete(); + emit sendComplete(false /* error */); } void APMGeoFenceManager::loadFromVehicle(void) @@ -332,5 +332,5 @@ void APMGeoFenceManager::removeAll(void) QmlObjectListModel emptyPolygon; sendToVehicle(_breachReturnPoint, emptyPolygon); - emit removeAllComplete(); + emit removeAllComplete(false /* error */); } diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc index 6adef3d606b47787ccf3d6850f910edc03347c90..3e1044aad408c2d874131b5c8626c8762c1670bb 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -48,7 +48,7 @@ void APMRallyPointManager::sendToVehicle(const QList& rgPoints) _sendRallyPoint(index); } - emit sendComplete(); + emit sendComplete(false /* error */); } void APMRallyPointManager::loadFromVehicle(void) @@ -157,5 +157,5 @@ void APMRallyPointManager::removeAll(void) QList noRallyPoints; sendToVehicle(noRallyPoints); - emit removeAllComplete(); + emit removeAllComplete(false /* error */); } diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index d3f4b284cd8d63fed535022d374295206e1d364f..e09c4fb72988efb1acd3c0aa70cab675a1bc1045 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -291,18 +291,20 @@ void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn _itemsRequested = false; } -void GeoFenceController::_managerSendComplete(void) +void GeoFenceController::_managerSendComplete(bool error) { // Fly view always reloads on manager sendComplete - if (!_editMode) { + if (!error && !_editMode) { showPlanFromManagerVehicle(); } } -void GeoFenceController::_managerRemoveAllComplete(void) +void GeoFenceController::_managerRemoveAllComplete(bool error) { - // Remove all from vehicle so we always update - showPlanFromManagerVehicle(); + if (!error) { + // Remove all from vehicle so we always update + showPlanFromManagerVehicle(); + } } bool GeoFenceController::containsItems(void) const diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h index 3e8b01aeba60a46563a822c8bcd32932e3c6c6bd..7019cba3133a79e6a7f0c63c63c7b3492e990f6b 100644 --- a/src/MissionManager/GeoFenceController.h +++ b/src/MissionManager/GeoFenceController.h @@ -90,8 +90,8 @@ private slots: void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint); void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList& polygon); void _updateContainsItems(void); - void _managerSendComplete(void); - void _managerRemoveAllComplete(void); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); private: void _init(void); diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc index 397262b49c86b094439c08b03c9064a1b68db6ce..357e3b768b335c2aa7f75bdfd2a76c74eadf1a59 100644 --- a/src/MissionManager/GeoFenceManager.cc +++ b/src/MissionManager/GeoFenceManager.cc @@ -43,12 +43,12 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjec // No geofence support in unknown vehicle Q_UNUSED(breachReturn); Q_UNUSED(polygon); - emit sendComplete(); + emit sendComplete(false /* error */); } void GeoFenceManager::removeAll(void) { // No geofence support in unknown vehicle - emit removeAllComplete(); + emit removeAllComplete(false /* error */); } diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h index d78fc499e94c0698e6060ccbe1b93d6bab66c215..899b2a32aca189b202da8a3f9045c1adb923582a 100644 --- a/src/MissionManager/GeoFenceManager.h +++ b/src/MissionManager/GeoFenceManager.h @@ -96,8 +96,8 @@ signals: void polygonSupportedChanged (bool polygonSupported); void polygonEnabledChanged (bool polygonEnabled); void breachReturnSupportedChanged (bool breachReturnSupported); - void removeAllComplete (void); - void sendComplete (void); + void removeAllComplete (bool error); + void sendComplete (bool error); protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 61ae05f372a85504545fac64c62a3391c336c98c..5067ac73c07f57422fb2792319a8230761349e9a 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1702,16 +1702,18 @@ bool MissionController::showPlanFromManagerVehicle (void) } } -void MissionController::_managerSendComplete(void) +void MissionController::_managerSendComplete(bool error) { - // FLy view always reloads on send complete - if (!_editMode) { + // Fly view always reloads on send complete + if (!error && !_editMode) { showPlanFromManagerVehicle(); } } -void MissionController::_managerRemoveAllComplete(void) +void MissionController::_managerRemoveAllComplete(bool error) { - // Remove all from vehicle so we always update - showPlanFromManagerVehicle(); + if (!error) { + // Remove all from vehicle so we always update + showPlanFromManagerVehicle(); + } } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 9e22b38bdd6e416939922e23e1ca03ce713cfd2e..b53a9de865c9d23a75edfb0c959b0910c3d2662c 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -176,8 +176,8 @@ private slots: void _updateContainsItems(void); void _progressPctChanged(double progressPct); void _visualItemsDirtyChanged(bool dirty); - void _managerSendComplete(void); - void _managerRemoveAllComplete(void); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); private: void _init(void); diff --git a/src/MissionManager/MissionControllerManagerTest.cc b/src/MissionManager/MissionControllerManagerTest.cc index b0cfceface58ea06db7e2220c592cab30543b5f3..22669624f64975d06918a33bde72759aeb127863 100644 --- a/src/MissionManager/MissionControllerManagerTest.cc +++ b/src/MissionManager/MissionControllerManagerTest.cc @@ -36,7 +36,7 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy QVERIFY(_missionManager); _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(bool)); - _rgMissionManagerSignals[sendCompleteSignalIndex] = SIGNAL(sendComplete(void)); + _rgMissionManagerSignals[sendCompleteSignalIndex] = SIGNAL(sendComplete(bool)); _rgMissionManagerSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); _rgMissionManagerSignals[errorSignalIndex] = SIGNAL(error(int, const QString&)); diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index c45b29cb4b0dc09f8549ee3950fd89e1c2793b31..b5e4be6603e4944a165b55c2b94a6719250b24fe 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -50,10 +50,10 @@ void MissionManager::_writeMissionItemsWorker(void) emit progressPct(0); - qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); + qCDebug(MissionManagerLog) << "writeMissionItems count:" << _writeMissionItems.count(); // Prime write list - for (int i=0; i<_missionItems.count(); i++) { + for (int i=0; i<_writeMissionItems.count(); i++) { _itemIndicesToWrite << i; } @@ -80,15 +80,15 @@ void MissionManager::writeMissionItems(const QList& missionItems) return; } - bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); + _clearAndDeleteWriteMissionItems(); - _clearAndDeleteMissionItems(); + bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); int firstIndex = skipFirstItem ? 1 : 0; for (int i=firstIndex; isetIsCurrentItem(i == firstIndex); @@ -107,7 +107,7 @@ void MissionManager::writeMissionItems(const QList& missionItems) /// This begins the write sequence with the vehicle. This may be called during a retry. void MissionManager::_writeMissionCount(void) { - qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _missionItems.count() << _retryCount; + qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _writeMissionItems.count() << _retryCount; mavlink_message_t message; mavlink_mission_count_t missionCount; @@ -115,7 +115,7 @@ void MissionManager::_writeMissionCount(void) memset(&missionCount, 0, sizeof(missionCount)); missionCount.target_system = _vehicle->id(); missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; - missionCount.count = _missionItems.count(); + missionCount.count = _writeMissionItems.count(); _dedicatedLink = _vehicle->priorityLink(); mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), @@ -539,13 +539,13 @@ 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)); + if (missionRequest.seq > _writeMissionItems.count() - 1) { + _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq)); _finishTransaction(false); return; } - emit progressPct((double)missionRequest.seq / (double)_missionItems.count()); + emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count()); _lastMissionRequest = missionRequest.seq; if (!_itemIndicesToWrite.contains(missionRequest.seq)) { @@ -554,7 +554,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo _itemIndicesToWrite.removeOne(missionRequest.seq); } - MissionItem* item = _missionItems[missionRequest.seq]; + MissionItem* item = _writeMissionItems[missionRequest.seq]; qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command(); mavlink_message_t messageOut; @@ -653,11 +653,11 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; _finishTransaction(true); } else { - _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1. Vehicle only has partial list of mission items.").arg(_itemIndicesToWrite.count())); + _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count())); _finishTransaction(false); } } else { - _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); + _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); _finishTransaction(false); } break; @@ -746,8 +746,8 @@ QString MissionManager::_ackTypeToString(AckType_t ackType) QString MissionManager::_lastMissionReqestString(MAV_MISSION_RESULT result) { - if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _missionItems.count()) { - MissionItem* item = _missionItems[_lastMissionRequest]; + if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) { + MissionItem* item = _writeMissionItems[_lastMissionRequest]; switch (result) { case MAV_MISSION_UNSUPPORTED_FRAME: @@ -862,10 +862,21 @@ void MissionManager::_finishTransaction(bool success) emit newMissionItemsAvailable(false); break; case TransactionWrite: - emit sendComplete(); + if (success) { + // Write succeeded, update internal list to be current + _clearAndDeleteMissionItems(); + for (int i=0; i<_writeMissionItems.count(); i++) { + _missionItems.append(_writeMissionItems[i]); + } + _writeMissionItems.clear(); + } else { + // Write failed, throw out the write list + _clearAndDeleteWriteMissionItems(); + } + emit sendComplete(!success /* error */); break; case TransactionRemoveAll: - emit removeAllComplete(); + emit removeAllComplete(!success /* error */); break; default: break; @@ -1090,9 +1101,9 @@ void MissionManager::generateResumeMission(int resumeIndex) } // Send to vehicle - _clearAndDeleteMissionItems(); + _clearAndDeleteWriteMissionItems(); for (int i=0; ideleteLater(); + } + _writeMissionItems.clear(); +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 6dbc5230385bede1ff9ba70d178256b05876e99a..5b258d1b12a8ec5e72a6d6bfb8fc4389b530c5a0 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -89,8 +89,8 @@ signals: void lastCurrentIndexChanged(int lastCurrentIndex); void resumeMissionReady(void); void progressPct(double progressPercentPct); - void removeAllComplete (void); - void sendComplete (void); + void removeAllComplete (bool error); + void sendComplete (bool error); private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); @@ -131,6 +131,7 @@ private: void _writeMissionCount(void); void _writeMissionItemsWorker(void); void _clearAndDeleteMissionItems(void); + void _clearAndDeleteWriteMissionItems(void); QString _lastMissionReqestString(MAV_MISSION_RESULT result); void _removeAllWorker(void); @@ -150,7 +151,8 @@ private: QMutex _dataMutex; - QList _missionItems; + QList _missionItems; ///< Set of mission items on vehicle + QList _writeMissionItems; ///< Set of mission items currently being written to vehicle int _currentMissionIndex; int _lastCurrentIndex; }; diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index a4f3967d75285ebcf065eb5edbeb1fbe670a4b7a..c1d365c4ed9ed66ad23b32b12af3f70ee03d0bfc 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -203,18 +203,20 @@ void RallyPointController::_managerLoadComplete(const QList rgPo _itemsRequested = false; } -void RallyPointController::_managerSendComplete(void) +void RallyPointController::_managerSendComplete(bool error) { // Fly view always reloads after send - if (_editMode) { + if (!error && _editMode) { showPlanFromManagerVehicle(); } } -void RallyPointController::_managerRemoveAllComplete(void) +void RallyPointController::_managerRemoveAllComplete(bool error) { - // Remove all from vehicle so we always update - showPlanFromManagerVehicle(); + if (!error) { + // Remove all from vehicle so we always update + showPlanFromManagerVehicle(); + } } void RallyPointController::addPoint(QGeoCoordinate point) diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h index 786f67f1704a95933b1c974174a3c1f1907cfb97..7a2c945817ebe07d43ca7620d8c6439578a11534 100644 --- a/src/MissionManager/RallyPointController.h +++ b/src/MissionManager/RallyPointController.h @@ -64,8 +64,8 @@ signals: private slots: void _managerLoadComplete(const QList rgPoints); - void _managerSendComplete(void); - void _managerRemoveAllComplete(void); + void _managerSendComplete(bool error); + void _managerRemoveAllComplete(bool error); void _setFirstPointCurrent(void); void _updateContainsItems(void); diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc index 2d1a391d855be918ffb0e2785e24612124432a49..d101b4e890bb78fa30cc00b8aac8814b5c77ebe4 100644 --- a/src/MissionManager/RallyPointManager.cc +++ b/src/MissionManager/RallyPointManager.cc @@ -41,11 +41,11 @@ void RallyPointManager::sendToVehicle(const QList& rgPoints) { // No support in generic vehicle Q_UNUSED(rgPoints); - emit sendComplete(); + emit sendComplete(false /* error */); } void RallyPointManager::removeAll(void) { // No support in generic vehicle - emit removeAllComplete(); + emit removeAllComplete(false /* error */); } diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h index a3f25337b7be83af38c6bd5a8a985b29fa4de163..db46f18fe39cea75b0b9503959d20c621f50337d 100644 --- a/src/MissionManager/RallyPointManager.h +++ b/src/MissionManager/RallyPointManager.h @@ -62,8 +62,8 @@ signals: void loadComplete (const QList rgPoints); void inProgressChanged (bool inProgress); void error (int errorCode, const QString& errorMsg); - void removeAllComplete (void); - void sendComplete (void); + void removeAllComplete (bool error); + void sendComplete (bool error); protected: void _sendError(ErrorCode_t errorCode, const QString& errorMsg);