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()
     }
@@ -219,19 +224,20 @@ Item {
             }
         }
 
-        readonly property int confirmHome:          1
-        readonly property int confirmLand:          2
-        readonly property int confirmTakeoff:       3
-        readonly property int confirmArm:           4
-        readonly property int confirmDisarm:        5
-        readonly property int confirmEmergencyStop: 6
-        readonly property int confirmChangeAlt:     7
-        readonly property int confirmGoTo:          8
-        readonly property int confirmRetask:        9
-        readonly property int confirmOrbit:         10
-        readonly property int confirmAbort:         11
-        readonly property int confirmStartMission:  12
-        readonly property int confirmResumeMission: 13
+        readonly property int confirmHome:                  1
+        readonly property int confirmLand:                  2
+        readonly property int confirmTakeoff:               3
+        readonly property int confirmArm:                   4
+        readonly property int confirmDisarm:                5
+        readonly property int confirmEmergencyStop:         6
+        readonly property int confirmChangeAlt:             7
+        readonly property int confirmGoTo:                  8
+        readonly property int confirmRetask:                9
+        readonly property int confirmOrbit:                 10
+        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.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,7 +82,9 @@ 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);
     void _ackTimeout(void);
@@ -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