diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index f027f2d857f106f028b7d0d023b6ec1973806c9f..2fa4cd40c7b41cfd58247add2d49097cdfd6ba59 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -1006,7 +1006,6 @@ void MissionManager::generateResumeMission(int resumeIndex) } resumeIndex = qMax(0, resumeIndex); - int seqNum = 0; QList resumeMission; QList 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; isetSequenceNumber(seqNum++); + } + int setCurrentIndex = addHomePosition ? 1 : 0; + resumeMission[setCurrentIndex]->setIsCurrentItem(true); + // Send to vehicle _clearAndDeleteWriteMissionItems(); for (int i=0; i