Merge pull request #5601 from DonLakeFlyer/SeqNum

Resume Mission: Fix up sequence numbers and current item

src/MissionManager/MissionManager.cc

@@ -1006,7 +1006,6 @@ void MissionManager::generateResumeMission(int resumeIndex)
     }
     resumeIndex = qMax(0, resumeIndex);
 
-    int seqNum = 0;
     QList<MissionItem*> resumeMission;
 
     QList<MAV_CMD> includedResumeCommands;
@@ -1029,7 +1028,6 @@ void MissionManager::generateResumeMission(int resumeIndex)
                            << MAV_CMD_NAV_TAKEOFF;
 
     bool addHomePosition = _vehicle->firmwarePlugin()->sendHomePositionToVehicle();
-    int setCurrentIndex = addHomePosition ? 1 : 0;
 
     int prefixCommandCount = 0;
     for (int i=0; i<_missionItems.count(); i++) {
@@ -1039,8 +1037,7 @@ void MissionManager::generateResumeMission(int resumeIndex)
                 prefixCommandCount++;
             }
             MissionItem* newItem = new MissionItem(*oldItem, this);
-            newItem->setIsCurrentItem( i == setCurrentIndex);
-            newItem->setSequenceNumber(seqNum++);
+            newItem->setIsCurrentItem(false);
             resumeMission.append(newItem);
         }
     }
@@ -1079,12 +1076,13 @@ void MissionManager::generateResumeMission(int resumeIndex)
             foundCameraStartStop = true;
             break;
         case MAV_CMD_IMAGE_START_CAPTURE:
-            // Only keep the first of these commands that are found
-            if (foundCameraStartStop) {
+            if (resumeItem->param3() != 0) {
+                // Remove commands which do not trigger by time
                 resumeMission.removeAt(prefixCommandCount);
+                break;
             }
-            if (resumeItem->param3() != 0) {
-                // Remove commands which do no trigger by time
+            if (foundCameraStartStop) {
+                // Only keep the first of these commands that are found
                 resumeMission.removeAt(prefixCommandCount);
             }
             foundCameraStartStop = true;
@@ -1096,6 +1094,14 @@ void MissionManager::generateResumeMission(int resumeIndex)
         prefixCommandCount--;
     }
 
+    // Adjust sequence numbers and current item
+    int seqNum = 0;
+    for (int i=0; i<resumeMission.count(); i++) {
+        resumeMission[i]->setSequenceNumber(seqNum++);
+    }
+    int setCurrentIndex = addHomePosition ? 1 : 0;
+    resumeMission[setCurrentIndex]->setIsCurrentItem(true);
+
     // Send to vehicle
     _clearAndDeleteWriteMissionItems();
     for (int i=0; i<resumeMission.count(); i++) {