Merge pull request #5592 from DonLakeFlyer/ResumeMission

Resume mission fixes

src/FlightDisplay/GuidedActionsController.qml

@@ -125,6 +125,8 @@ Item {
     property bool __pauseVehicleSupported: _activeVehicle ? _activeVehicle.pauseVehicleSupported : false
     property bool __flightMode: _flightMode
 
+    /*
+      Leaving this code in for use while debugging
     function _outputState() {
         console.log(qsTr("_activeVehicle(%1) _vehicleArmed(%2) guidedModeSupported(%3) _vehicleFlying(%4) _vehicleInRTLMode(%5) pauseVehicleSupported(%6) _vehiclePaused(%7) _flightMode(%8)").arg(_activeVehicle ? 1 : 0).arg(_vehicleArmed ? 1 : 0).arg(__guidedModeSupported ? 1 : 0).arg(_vehicleFlying ? 1 : 0).arg(_vehicleInRTLMode ? 1 : 0).arg(__pauseVehicleSupported ? 1 : 0).arg(_vehiclePaused ? 1 : 0).arg(_flightMode))
     }
@@ -139,16 +141,16 @@ Item {
     on__PauseVehicleSupportedChanged: _outputState()
 
     // End of hack
+    */
 
     on_VehicleFlyingChanged: {
-        _outputState()
+        //_outputState()
         if (!_vehicleFlying) {
             // We use _vehicleWasFLying to help trigger Resume Mission only if the vehicle actually flew and came back down.
             // Otherwise it may trigger during the Start Mission sequence due to signal ordering or armed and resume mission index.
             _vehicleWasFlying = true
         }
     }
-
     property var    _actionData
 
     on_CurrentMissionIndexChanged: console.log("_currentMissionIndex", _currentMissionIndex)

src/MissionManager/MissionManager.cc

@@ -988,7 +988,23 @@ void MissionManager::generateResumeMission(int resumeIndex)
         }
     }
 
-    resumeIndex = qMin(resumeIndex, _missionItems.count() - 1);
+    // Be anal about crap input
+    resumeIndex = qMax(0, qMin(resumeIndex, _missionItems.count() - 1));
+
+    // Adjust resume index to be a location based command
+    const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, _missionItems[resumeIndex]->command());
+    if (!uiInfo || uiInfo->isStandaloneCoordinate() || !uiInfo->specifiesCoordinate()) {
+        // We have to back up to the last command which the vehicle flies through
+        while (--resumeIndex > 0) {
+            uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, _missionItems[resumeIndex]->command());
+            if (uiInfo && (uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate())) {
+                // Found it
+                break;
+            }
+
+        }
+    }
+    resumeIndex = qMax(0, resumeIndex);
 
     int seqNum = 0;
     QList<MissionItem*> resumeMission;
@@ -1008,20 +1024,19 @@ void MissionManager::generateResumeMission(int resumeIndex)
                            << MAV_CMD_IMAGE_STOP_CAPTURE
                            << MAV_CMD_VIDEO_START_CAPTURE
                            << MAV_CMD_VIDEO_STOP_CAPTURE
-                           << MAV_CMD_DO_CHANGE_SPEED;
-    if (_vehicle->fixedWing() && _vehicle->px4Firmware()) {
-        includedResumeCommands << MAV_CMD_NAV_TAKEOFF;
-    }
+                           << MAV_CMD_DO_CHANGE_SPEED
+                           << MAV_CMD_SET_CAMERA_MODE
+                           << MAV_CMD_NAV_TAKEOFF;
 
     bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
     int setCurrentIndex = addHomePosition ? 1 : 0;
 
-    int resumeCommandCount = 0;
+    int prefixCommandCount = 0;
     for (int i=0; i<_missionItems.count(); i++) {
         MissionItem* oldItem = _missionItems[i];
         if ((i == 0 && addHomePosition) || i >= resumeIndex || includedResumeCommands.contains(oldItem->command())) {
             if (i < resumeIndex) {
-                resumeCommandCount++;
+                prefixCommandCount++;
             }
             MissionItem* newItem = new MissionItem(*oldItem, this);
             newItem->setIsCurrentItem( i == setCurrentIndex);
@@ -1029,91 +1044,56 @@ void MissionManager::generateResumeMission(int resumeIndex)
             resumeMission.append(newItem);
         }
     }
+    prefixCommandCount = qMax(0, qMin(prefixCommandCount, resumeMission.count()));  // Anal prevention against crashes
 
     // De-dup and remove no-ops from the commands which were added to the front of the mission
     bool foundROI = false;
-    bool foundCamTrigDist = false;
-    QList<int> imageStartCameraIds;
-    QList<int> imageStopCameraIds;
-    QList<int> videoStartCameraIds;
-    QList<int> videoStopCameraIds;
-    while (resumeIndex >= 0) {
-        MissionItem* resumeItem = resumeMission[resumeIndex];
+    bool foundCameraSetMode = false;
+    bool foundCameraStartStop = false;
+    prefixCommandCount--;   // Change from count to array index
+    while (prefixCommandCount >= 0) {
+        MissionItem* resumeItem = resumeMission[prefixCommandCount];
         switch (resumeItem->command()) {
+        case MAV_CMD_SET_CAMERA_MODE:
+            // Only keep the last one
+            if (foundCameraSetMode) {
+                resumeMission.removeAt(prefixCommandCount);
+            }
+            foundCameraSetMode = true;
+            break;
         case MAV_CMD_DO_SET_ROI:
             // Only keep the last one
             if (foundROI) {
-                resumeMission.removeAt(resumeIndex);
+                resumeMission.removeAt(prefixCommandCount);
             }
             foundROI = true;
             break;
         case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
-            // Only keep the last one
-            if (foundCamTrigDist) {
-                resumeMission.removeAt(resumeIndex);
-            }
-            foundCamTrigDist = true;
-            break;
-        case MAV_CMD_IMAGE_START_CAPTURE:
-        {
-            // FIXME: Handle single image capture
-            int cameraId = resumeItem->param1();
-
-            if (resumeItem->param3() == 1) {
-                // This is an individual image capture command, remove it
-                resumeMission.removeAt(resumeIndex);
-                break;
-            }
-            // If we already found an image stop, then all image start/stop commands are useless
-            // De-dup repeated image start commands
-            // Otherwise keep only the last image start
-            if (imageStopCameraIds.contains(cameraId) || imageStartCameraIds.contains(cameraId)) {
-                resumeMission.removeAt(resumeIndex);
-            }
-            if (!imageStopCameraIds.contains(cameraId)) {
-                imageStopCameraIds.append(cameraId);
-            }
-        }
-            break;
         case MAV_CMD_IMAGE_STOP_CAPTURE:
-        {
-            int cameraId = resumeItem->param1();
-            // Image stop only matters to kill all previous image starts
-            if (!imageStopCameraIds.contains(cameraId)) {
-                imageStopCameraIds.append(cameraId);
-            }
-            resumeMission.removeAt(resumeIndex);
-        }
-            break;
         case MAV_CMD_VIDEO_START_CAPTURE:
-        {
-            int cameraId = resumeItem->param1();
-            // If we've already found a video stop, then all video start/stop commands are useless
-            // De-dup repeated video start commands
-            // Otherwise keep only the last video start
-            if (videoStopCameraIds.contains(cameraId) || videoStopCameraIds.contains(cameraId)) {
-                resumeMission.removeAt(resumeIndex);
-            }
-            if (!videoStopCameraIds.contains(cameraId)) {
-                videoStopCameraIds.append(cameraId);
-            }
+        case MAV_CMD_VIDEO_STOP_CAPTURE:
+            // Only keep the first of these commands that are found
+            if (foundCameraStartStop) {
+                resumeMission.removeAt(prefixCommandCount);
             }
+            foundCameraStartStop = true;
             break;
-        case MAV_CMD_VIDEO_STOP_CAPTURE:
-        {
-            int cameraId = resumeItem->param1();
-            // Video stop only matters to kill all previous video starts
-            if (!videoStopCameraIds.contains(cameraId)) {
-                videoStopCameraIds.append(cameraId);
+        case MAV_CMD_IMAGE_START_CAPTURE:
+            // Only keep the first of these commands that are found
+            if (foundCameraStartStop) {
+                resumeMission.removeAt(prefixCommandCount);
             }
-            resumeMission.removeAt(resumeIndex);
+            if (resumeItem->param3() != 0) {
+                // Remove commands which do no trigger by time
+                resumeMission.removeAt(prefixCommandCount);
             }
+            foundCameraStartStop = true;
             break;
         default:
             break;
         }
 
-        resumeIndex--;
+        prefixCommandCount--;
     }
 
     // Send to vehicle