Add resume mission support

src/FlightDisplay/FlightDisplayViewWidgets.qml

@@ -38,7 +38,7 @@ Item {
     // Guided bar properties
     property bool _missionAvailable:    missionController.containsItems
     property bool _missionActive:       _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.missionFlightMode : false
-    property bool _missionInProgress:   missionController.missionInProgress
+    property int  _resumeMissionItem:   missionController.resumeMissionItem
     property bool _showEmergenyStop:    QGroundControl.corePlugin.options.guidedBarShowEmergencyStop
     property bool _showOrbit:           QGroundControl.corePlugin.options.guidedBarShowOrbit
 
@@ -92,6 +92,11 @@ Item {
         onValueChanged: _setInstrumentWidget()
     }
 
+    Connections {
+        target:                 missionController
+        onResumeMissionReady:   _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMissionReady)
+    }
+
     Component.onCompleted: {
         _setInstrumentWidget()
     }
@@ -232,6 +237,7 @@ Item {
         readonly property int confirmAbort:                 11
         readonly property int confirmStartMission:          12
         readonly property int confirmResumeMission:         13
+        readonly property int confirmResumeMissionReady:    14
 
         property int    confirmActionCode
         property real   _showMargin:    _margins
@@ -250,6 +256,11 @@ Item {
                 _activeVehicle.guidedModeTakeoff()
                 break;
             case confirmResumeMission:
+                missionController.resumeMission(missionController.resumeMissionItem)
+                break;
+            case confirmResumeMissionReady:
+                _activeVehicle.startMission()
+                break;
             case confirmStartMission:
                 _activeVehicle.startMission()
                 break;
@@ -318,6 +329,9 @@ Item {
             case confirmResumeMission:
                 guidedModeConfirm.confirmText = qsTr("resume mission")
                 break;
+            case confirmResumeMissionReady:
+                guidedModeConfirm.confirmText = qsTr("resume modified mission after review")
+                break;
             case confirmLand:
                 guidedModeConfirm.confirmText = qsTr("land")
                 break;
@@ -400,15 +414,15 @@ Item {
 
                 QGCButton {
                     pointSize:  _guidedModeBar._fontPointSize
-                    text:       qsTr("Start Mission")
+                    text:       _resumeMissionItem !== -1 ? qsTr("Restart Mission") : qsTr("Start Mission")
                     visible:    _activeVehicle && !_activeVehicle.flying && _missionAvailable
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmStartMission)
                 }
 
                 QGCButton {
                     pointSize:  _guidedModeBar._fontPointSize
-                    text:       qsTr("Resume Mission")
-                    visible:    _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && _missionAvailable && _missionInProgress
+                    text:       qsTr("Resume Mission (%1)").arg(_resumeMissionItem)
+                    visible:    _activeVehicle && !_activeVehicle.flying && _missionAvailable && _resumeMissionItem !== -1
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMission)
                 }
 
@@ -422,14 +436,14 @@ Item {
                 QGCButton {
                     pointSize:  _guidedModeBar._fontPointSize
                     text:       qsTr("Change Altitude")
-                    visible:    (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed
+                    visible:    (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_missionActive
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt)
                 }
 
                 QGCButton {
                     pointSize:  _guidedModeBar._fontPointSize
                     text:       qsTr("Orbit")
-                    visible:    _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed
+                    visible:    _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed && !_missionActive
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
                 }
 

src/MissionManager/MissionController.cc

@@ -53,7 +53,6 @@ MissionController::MissionController(QObject *parent)
     , _settingsItem(NULL)
     , _firstItemsFromVehicle(false)
     , _missionItemsRequested(false)
-    , _queuedSend(false)
     , _surveyMissionItemName(tr("Survey"))
     , _fwLandingMissionItemName(tr("Fixed Wing Landing"))
     , _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings())
@@ -647,7 +646,6 @@ void MissionController::loadFromFile(const QString& filename)
     MissionController::_scanForAdditionalSettings(_visualItems, _activeVehicle);
 
     _initAllVisualItems();
-    sendToVehicle();
 }
 
 bool MissionController::loadItemsFromFile(Vehicle* vehicle, const QString& filename, QmlObjectListModel** visualItems)
@@ -1280,6 +1278,8 @@ void MissionController::_activeVehicleBeingRemoved(void)
     disconnect(missionManager, &MissionManager::newMissionItemsAvailable,   this, &MissionController::_newMissionItemsAvailableFromVehicle);
     disconnect(missionManager, &MissionManager::inProgressChanged,          this, &MissionController::_inProgressChanged);
     disconnect(missionManager, &MissionManager::currentItemChanged,         this, &MissionController::_currentMissionItemChanged);
+    disconnect(missionManager, &MissionManager::lastCurrentItemChanged,     this, &MissionController::resumeMissionItemChanged);
+    disconnect(missionManager, &MissionManager::resumeMissionReady,         this, &MissionController::resumeMissionReady);
     disconnect(_activeVehicle, &Vehicle::homePositionChanged,               this, &MissionController::_activeVehicleHomePositionChanged);
 
     // We always remove all items on vehicle change. This leaves a user model hole:
@@ -1298,6 +1298,8 @@ void MissionController::_activeVehicleSet(void)
     connect(missionManager, &MissionManager::newMissionItemsAvailable,  this, &MissionController::_newMissionItemsAvailableFromVehicle);
     connect(missionManager, &MissionManager::inProgressChanged,         this, &MissionController::_inProgressChanged);
     connect(missionManager, &MissionManager::currentItemChanged,        this, &MissionController::_currentMissionItemChanged);
+    connect(missionManager, &MissionManager::lastCurrentItemChanged,    this, &MissionController::resumeMissionItemChanged);
+    connect(missionManager, &MissionManager::resumeMissionReady,        this, &MissionController::resumeMissionReady);
     connect(_activeVehicle, &Vehicle::homePositionChanged,              this, &MissionController::_activeVehicleHomePositionChanged);
     connect(_activeVehicle, &Vehicle::defaultCruiseSpeedChanged,        this, &MissionController::_recalcMissionFlightStatus);
     connect(_activeVehicle, &Vehicle::defaultHoverSpeedChanged,         this, &MissionController::_recalcMissionFlightStatus);
@@ -1312,6 +1314,7 @@ void MissionController::_activeVehicleSet(void)
     _activeVehicleHomePositionChanged(_activeVehicle->homePosition());
 
     emit complexMissionItemNamesChanged();
+    emit resumeMissionItemChanged();
 }
 
 void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
@@ -1478,11 +1481,29 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel
     }
 }
 
-void MissionController::_currentMissionItemChanged(int sequenceNumber)
+int MissionController::resumeMissionItem(void) const
 {
+
+    int resumeIndex = -1;
+
     if (!_editMode) {
-        bool prevMissionInProgress = missionInProgress();
+        int firstTrueItemIndex = _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() ? 1 : 0;
+        resumeIndex = _activeVehicle->missionManager()->lastCurrentItem();
+        if (resumeIndex > firstTrueItemIndex) {
+            if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
+                resumeIndex++;
+            }
+            // Resume at the item previous to the item we were heading towards
+            resumeIndex--;
+        }
+    }
+
+    return resumeIndex;
+}
 
+void MissionController::_currentMissionItemChanged(int sequenceNumber)
+{
+    if (!_editMode) {
         if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
             sequenceNumber++;
         }
@@ -1491,10 +1512,6 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber)
             VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
             item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
         }
-
-        if (prevMissionInProgress != missionInProgress()) {
-            emit missionInProgressChanged();
-        }
     }
 }
 
@@ -1576,11 +1593,6 @@ QStringList MissionController::complexMissionItemNames(void) const
     return complexItems;
 }
 
-bool MissionController::missionInProgress(void) const
-{
-    return _visualItems && _visualItems->count() > 1 && (!_visualItems->value<VisualMissionItem*>(0)->isCurrentItem() && !_visualItems->value<VisualMissionItem*>(1)->isCurrentItem());
-}
-
 void MissionController::_visualItemsDirtyChanged(bool dirty)
 {
     if (dirty) {
@@ -1594,3 +1606,11 @@ void MissionController::_visualItemsDirtyChanged(bool dirty)
         emit dirtyChanged(false);
     }
 }
+
+void MissionController::resumeMission(int resumeIndex)
+{
+    if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
+        resumeIndex--;
+    }
+    _activeVehicle->missionManager()->generateResumeMission(resumeIndex);
+}

src/MissionManager/MissionController.h

@@ -55,7 +55,7 @@ public:
     Q_PROPERTY(QmlObjectListModel*  waypointLines           READ waypointLines              NOTIFY waypointLinesChanged)
     Q_PROPERTY(QStringList          complexMissionItemNames READ complexMissionItemNames    NOTIFY complexMissionItemNamesChanged)
 
-    Q_PROPERTY(bool                 missionInProgress       READ missionInProgress          NOTIFY missionInProgressChanged)        ///< true: Mission sequence is beyond first item
+    Q_PROPERTY(int                  resumeMissionItem       READ resumeMissionItem          NOTIFY resumeMissionItemChanged)
 
     Q_PROPERTY(double               missionDistance         READ missionDistance            NOTIFY missionDistanceChanged)
     Q_PROPERTY(double               missionTime             READ missionTime                NOTIFY missionTimeChanged)
@@ -79,6 +79,8 @@ public:
     /// @return Sequence number for new item
     Q_INVOKABLE int insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int i);
 
+    Q_INVOKABLE void resumeMission(int resumeIndex);
+
     /// Loads the mission items from the specified file
     ///     @param[in] vehicle Vehicle we are loading items for
     ///     @param[in] filename File to load from
@@ -110,7 +112,9 @@ public:
     QmlObjectListModel* visualItems             (void) { return _visualItems; }
     QmlObjectListModel* waypointLines           (void) { return &_waypointLines; }
     QStringList         complexMissionItemNames (void) const;
-    bool                missionInProgress       (void) const;
+
+    /// Returns the item index two which a mission should be resumed. -1 indicates resume mission not available.
+    int resumeMissionItem(void) const;
 
     double  missionDistance         (void) const { return _missionFlightStatus.totalDistance; }
     double  missionTime             (void) const { return _missionFlightStatus.totalTime; }
@@ -132,7 +136,8 @@ signals:
     void missionCruiseTimeChanged(void);
     void missionMaxTelemetryChanged(double missionMaxTelemetry);
     void complexMissionItemNamesChanged(void);
-    bool missionInProgressChanged(void);
+    void resumeMissionItemChanged(void);
+    void resumeMissionReady(void);
 
 private slots:
     void _newMissionItemsAvailableFromVehicle(bool removeAllRequested);
@@ -188,7 +193,6 @@ private:
     CoordVectHashTable      _linesTable;
     bool                    _firstItemsFromVehicle;
     bool                    _missionItemsRequested;
-    bool                    _queuedSend;
     MissionFlightStatus_t   _missionFlightStatus;
     QString                 _surveyMissionItemName;
     QString                 _fwLandingMissionItemName;

src/MissionManager/MissionManager.cc

@@ -26,7 +26,9 @@ MissionManager::MissionManager(Vehicle* vehicle)
     , _expectedAck(AckNone)
     , _readTransactionInProgress(false)
     , _writeTransactionInProgress(false)
+    , _resumeMission(false)
     , _currentMissionItem(-1)
+    , _lastCurrentItem(-1)
 {
     connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
     
@@ -42,6 +44,29 @@ MissionManager::~MissionManager()
 
 }
 
+void MissionManager::_writeMissionItemsWorker(void)
+{
+    emit newMissionItemsAvailable(_missionItems.count() == 0);
+
+    qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
+
+    // Prime write list
+    for (int i=0; i<_missionItems.count(); i++) {
+        _itemIndicesToWrite << i;
+    }
+
+    _writeTransactionInProgress = true;
+    _retryCount = 0;
+    emit inProgressChanged(true);
+    _writeMissionCount();
+
+    _currentMissionItem = -1;
+    _lastCurrentItem = -1;
+    emit currentItemChanged(-1);
+    emit lastCurrentItemChanged(-1);
+}
+
+
 void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
 {
     if (_vehicle->isOfflineEditingVehicle()) {
@@ -55,7 +80,7 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
 
     bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
 
-    _missionItems.clear();
+    _clearAndDeleteMissionItems();
 
     int firstIndex = skipFirstItem ? 1 : 0;
 
@@ -73,19 +98,8 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
             }
         }
     }
-    emit newMissionItemsAvailable(missionItems.count() == 0);
-
-    qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
-    
-    // Prime write list
-    for (int i=0; i<_missionItems.count(); i++) {
-        _itemIndicesToWrite << i;
-    }
 
-    _writeTransactionInProgress = true;
-    _retryCount = 0;
-    emit inProgressChanged(true);
-    _writeMissionCount();
+    _writeMissionItemsWorker();
 }
 
 /// This begins the write sequence with the vehicle. This may be called during a retry.
@@ -470,7 +484,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
 void MissionManager::_clearMissionItems(void)
 {
     _itemIndicesToRead.clear();
-    _missionItems.clear();
+    _clearAndDeleteMissionItems();
 }
 
 void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
@@ -736,7 +750,7 @@ void MissionManager::_finishTransaction(bool success)
 {
     if (!success && _readTransactionInProgress) {
         // Read from vehicle failed, clear partial list
-        _missionItems.clear();
+        _clearAndDeleteMissionItems();
         emit newMissionItemsAvailable(false);
     }
 
@@ -748,6 +762,11 @@ void MissionManager::_finishTransaction(bool success)
         _writeTransactionInProgress = false;
         emit inProgressChanged(false);
     }
+
+    if (_resumeMission) {
+        _resumeMission = false;
+        emit resumeMissionReady();
+    }
 }
 
 bool MissionManager::inProgress(void)
@@ -766,6 +785,11 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
         _currentMissionItem = missionCurrent.seq;
         emit currentItemChanged(_currentMissionItem);
     }
+
+    if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionItem != _lastCurrentItem) {
+        _lastCurrentItem = _currentMissionItem;
+        emit lastCurrentItemChanged(_lastCurrentItem);
+    }
 }
 
 void MissionManager::removeAll(void)
@@ -774,3 +798,70 @@ void MissionManager::removeAll(void)
 
     writeMissionItems(emptyList);
 }
+
+void MissionManager::generateResumeMission(int resumeIndex)
+{
+    if (_vehicle->isOfflineEditingVehicle()) {
+        return;
+    }
+
+    if (inProgress()) {
+        qCDebug(MissionManagerLog) << "generateResumeMission called while transaction in progress";
+        return;
+    }
+
+    int seqNum = 0;
+    QList<MissionItem*> resumeMission;
+
+    QList<MAV_CMD> includedResumeCommands;
+
+    // If any command in this list occurs before the resumeIndex it will be added to the front of the mission
+    includedResumeCommands << MAV_CMD_DO_CONTROL_VIDEO
+                           << MAV_CMD_DO_SET_ROI
+                           << MAV_CMD_DO_DIGICAM_CONFIGURE
+                           << MAV_CMD_DO_DIGICAM_CONTROL
+                           << MAV_CMD_DO_MOUNT_CONFIGURE
+                           << MAV_CMD_DO_MOUNT_CONTROL
+                           << MAV_CMD_DO_SET_CAM_TRIGG_DIST
+                           << MAV_CMD_DO_FENCE_ENABLE
+                           << MAV_CMD_IMAGE_START_CAPTURE
+                           << MAV_CMD_IMAGE_STOP_CAPTURE
+                           << MAV_CMD_VIDEO_START_CAPTURE
+                           << MAV_CMD_VIDEO_STOP_CAPTURE;
+
+    bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
+    int setCurrentIndex = addHomePosition ? 1 : 0;
+
+    for (int i=0; i<_missionItems.count(); i++) {
+        MissionItem* oldItem = _missionItems[i];
+        if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) {
+            MissionItem* newItem = new MissionItem(*oldItem, this);
+            newItem->setIsCurrentItem( i == setCurrentIndex);
+            newItem->setSequenceNumber(seqNum++);
+            resumeMission.append(newItem);
+        }
+    }
+
+    // Handle DO_JUMP seq num update
+
+    // Send to vehicle
+    _clearAndDeleteMissionItems();
+    for (int i=0; i<resumeMission.count(); i++) {
+        _missionItems.append(new MissionItem(*resumeMission[i], this));
+    }
+    _resumeMission = true;
+    _writeMissionItemsWorker();
+
+    // Clean up no longer needed resume items
+    for (int i=0; i<resumeMission.count(); i++) {
+        resumeMission[i]->deleteLater();
+    }
+}
+
+void MissionManager::_clearAndDeleteMissionItems(void)
+{
+    for (int i=0; i<_missionItems.count(); i++) {
+        _missionItems[i]->deleteLater();
+    }
+    _missionItems.clear();
+}

src/MissionManager/MissionManager.h

@@ -36,7 +36,12 @@ public:
     
     bool inProgress(void);
     const QList<MissionItem*>& missionItems(void) { return _missionItems; }
-    int currentItem(void) { return _currentMissionItem; }
+
+    /// Current mission item as reported by MISSION_CURRENT
+    int currentItem(void) const { return _currentMissionItem; }
+
+    /// Last current mission item reported while in Mission flight mode
+    int lastCurrentItem(void) const { return _lastCurrentItem; }
     
     void requestMissionItems(void);
     
@@ -52,6 +57,10 @@ public:
     /// Removes all mission items from vehicle
     void removeAll(void);
 
+    /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
+    /// from mission start to resumeIndex in the generate mission.
+    void generateResumeMission(int resumeIndex);
+
     /// Error codes returned in error signal
     typedef enum {
         InternalError,
@@ -73,6 +82,8 @@ signals:
     void inProgressChanged(bool inProgress);
     void error(int errorCode, const QString& errorMsg);
     void currentItemChanged(int currentItem);
+    void lastCurrentItemChanged(int lastCurrentMissionItem);
+    void resumeMissionReady(void);
 
 private slots:
     void _mavlinkMessageReceived(const mavlink_message_t& message);
@@ -103,6 +114,8 @@ private:
     void _finishTransaction(bool success);
     void _requestList(void);
     void _writeMissionCount(void);
+    void _writeMissionItemsWorker(void);
+    void _clearAndDeleteMissionItems(void);
 
 private:
     Vehicle*            _vehicle;
@@ -114,6 +127,7 @@ private:
     
     bool        _readTransactionInProgress;
     bool        _writeTransactionInProgress;
+    bool        _resumeMission;
     QList<int>  _itemIndicesToWrite;    ///< List of mission items which still need to be written to vehicle
     QList<int>  _itemIndicesToRead;     ///< List of mission items which still need to be requested from vehicle
     
@@ -121,6 +135,7 @@ private:
     
     QList<MissionItem*> _missionItems;
     int                 _currentMissionItem;
+    int                 _lastCurrentItem;
 };
 
 #endif