Merge pull request #5074 from DonLakeFlyer/FailedUpload

Plan: Correctly handling of failed upload

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 */);
 }

src/FirmwarePlugin/APM/APMRallyPointManager.cc

@@ -48,7 +48,7 @@ void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
         _sendRallyPoint(index);
     }
 
-    emit sendComplete();
+    emit sendComplete(false /* error */);
 }
 
 void APMRallyPointManager::loadFromVehicle(void)
@@ -157,5 +157,5 @@ void APMRallyPointManager::removeAll(void)
     QList<QGeoCoordinate> noRallyPoints;
 
     sendToVehicle(noRallyPoints);
-    emit removeAllComplete();
+    emit removeAllComplete(false /* error */);
 }

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

src/MissionManager/GeoFenceController.h

@@ -90,8 +90,8 @@ private slots:
     void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
     void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
     void _updateContainsItems(void);
-    void _managerSendComplete(void);
-    void _managerRemoveAllComplete(void);
+    void _managerSendComplete(bool error);
+    void _managerRemoveAllComplete(bool error);
 
 private:
     void _init(void);

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 */);
 }

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

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();
+    }
 }

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

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

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<MissionItem*>& missionItems)
         return;
     }
 
-    bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
+    _clearAndDeleteWriteMissionItems();
 
-    _clearAndDeleteMissionItems();
+    bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
 
     int firstIndex = skipFirstItem ? 1 : 0;
 
     for (int i=firstIndex; i<missionItems.count(); i++) {
         MissionItem* item = new MissionItem(*missionItems[i]);
-        _missionItems.append(item);
+        _writeMissionItems.append(item);
 
         item->setIsCurrentItem(i == firstIndex);
 
@@ -107,7 +107,7 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& 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; i<resumeMission.count(); i++) {
-        _missionItems.append(new MissionItem(*resumeMission[i], this));
+        _writeMissionItems.append(new MissionItem(*resumeMission[i], this));
     }
     _resumeMission = true;
     _writeMissionItemsWorker();
@@ -1110,3 +1121,12 @@ void MissionManager::_clearAndDeleteMissionItems(void)
     }
     _missionItems.clear();
 }
+
+
+void MissionManager::_clearAndDeleteWriteMissionItems(void)
+{
+    for (int i=0; i<_writeMissionItems.count(); i++) {
+        _writeMissionItems[i]->deleteLater();
+    }
+    _writeMissionItems.clear();
+}

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<MissionItem*> _missionItems;
+    QList<MissionItem*> _missionItems;          ///< Set of mission items on vehicle
+    QList<MissionItem*> _writeMissionItems;     ///< Set of mission items currently being written to vehicle
     int                 _currentMissionIndex;
     int                 _lastCurrentIndex;
 };

src/MissionManager/RallyPointController.cc

@@ -203,18 +203,20 @@ void RallyPointController::_managerLoadComplete(const QList<QGeoCoordinate> 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)

src/MissionManager/RallyPointController.h

@@ -64,8 +64,8 @@ signals:
 
 private slots:
     void _managerLoadComplete(const QList<QGeoCoordinate> rgPoints);
-    void _managerSendComplete(void);
-    void _managerRemoveAllComplete(void);
+    void _managerSendComplete(bool error);
+    void _managerRemoveAllComplete(bool error);
     void _setFirstPointCurrent(void);
     void _updateContainsItems(void);
 

src/MissionManager/RallyPointManager.cc

@@ -41,11 +41,11 @@ void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& 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 */);
 }

src/MissionManager/RallyPointManager.h

@@ -62,8 +62,8 @@ signals:
     void loadComplete       (const QList<QGeoCoordinate> 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);