diff --git a/src/MissionEditor/MissionSettingsEditor.qml b/src/MissionEditor/MissionSettingsEditor.qml index 0cc47d950cd250e2a387fbeb7838cf6e6521b7f8..fdc26e8fa3af602487a990664d64f62ad52761ff 100644 --- a/src/MissionEditor/MissionSettingsEditor.qml +++ b/src/MissionEditor/MissionSettingsEditor.qml @@ -217,15 +217,12 @@ Rectangle { } } // GridLayout - /* - FIXME: NYI FactComboBox { anchors.left: parent.left anchors.right: parent.right fact: missionItem.missionEndAction indexModel: false } - */ } CameraSection { diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 518527976438d02ae398f21c0f2b5fc69d2017b8..b6888d4e9b737b1551e4812f732e5474e57c14f4 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -166,22 +166,46 @@ void MissionController::sendToVehicle(void) _visualItems->setDirty(false); } +/// Converts from visual items to MissionItems +/// @param missionItemParent QObject parent for newly allocated MissionItems +/// @return true: Mission end action was added to end of list +bool MissionController::_convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent) +{ + bool endActionSet = false; + int lastSeqNum = 0; + + for (int i=0; icount(); i++) { + VisualMissionItem* visualItem = qobject_cast(visualMissionItems->get(i)); + + lastSeqNum = visualItem->lastSequenceNumber(); + visualItem->appendMissionItems(rgMissionItems, missionItemParent); + + qCDebug(MissionControllerLog) << "_convertToMissionItems seqNum:lastSeqNum:command" + << visualItem->sequenceNumber() + << lastSeqNum + << visualItem->commandName(); + } + + // Mission settings has a special case for end mission action + MissionSettingsComplexItem* settingsItem = visualMissionItems->value(0); + if (settingsItem) { + endActionSet = settingsItem->addMissionEndAction(rgMissionItems, lastSeqNum + 1, missionItemParent); + } + + return endActionSet; +} + void MissionController::sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems) { if (vehicle) { - // Convert to MissionItems so we can send to vehicle - QList missionItems; - - for (int i=0; icount(); i++) { - VisualMissionItem* visualItem = qobject_cast(visualMissionItems->get(i)); + QList rgMissionItems; - visualItem->appendMissionItems(missionItems, NULL); - } + _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle); - vehicle->missionManager()->writeMissionItems(missionItems); + vehicle->missionManager()->writeMissionItems(rgMissionItems); - for (int i=0; ideleteLater(); + for (int i=0; ideleteLater(); } } } @@ -464,9 +488,9 @@ bool MissionController::_loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObje QString itemType = itemObject[VisualMissionItem::jsonTypeKey].toString(); if (itemType == VisualMissionItem::jsonTypeSimpleItemValue) { - qCDebug(MissionControllerLog) << "Loading MISSION_ITEM: nextSequenceNumber" << nextSequenceNumber; SimpleMissionItem* simpleItem = new SimpleMissionItem(vehicle, visualItems); if (simpleItem->load(itemObject, nextSequenceNumber, errorString)) { + qCDebug(MissionControllerLog) << "Loading simple item: nextSequenceNumber:command" << nextSequenceNumber << simpleItem->command(); nextSequenceNumber = simpleItem->lastSequenceNumber() + 1; visualItems->append(simpleItem); } else { @@ -709,12 +733,33 @@ void MissionController::saveToFile(const QString& filename) missionFileObject[_jsonHoverSpeedKey] = _activeVehicle->defaultHoverSpeed(); // Save the visual items - QJsonArray rgMissionItems; + + QJsonArray rgJsonMissionItems; + int lastSeqNum = 0; + for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* visualItem = qobject_cast(_visualItems->get(i)); - visualItem->save(rgMissionItems); + + visualItem->save(rgJsonMissionItems); + lastSeqNum = visualItem->lastSequenceNumber(); } - missionFileObject[_jsonItemsKey] = rgMissionItems; + + // Mission settings has a special case for end mission action + if (settingsItem) { + QList rgMissionItems; + + if (_convertToMissionItems(_visualItems, rgMissionItems, this /* missionItemParent */)) { + QJsonObject saveObject; + MissionItem* missionItem = rgMissionItems[rgMissionItems.count() - 1]; + missionItem->save(saveObject); + rgJsonMissionItems.append(saveObject); + } + for (int i=0; ideleteLater(); + } + } + + missionFileObject[_jsonItemsKey] = rgJsonMissionItems; QJsonDocument saveDoc(missionFileObject); file.write(saveDoc.toJson()); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 7a1de5bf9b9cd1048e5dd8acb1e0c7c92cd1c6af..9cc6689d72e2a23f9f708f0ee1b356475f44799a 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -21,6 +21,7 @@ class CoordinateVector; class VisualMissionItem; +class MissionItem; Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog) @@ -174,6 +175,7 @@ private: void _setMissionCruiseTime(double missionCruiseTime); void _setMissionMaxTelemetry(double missionMaxTelemetry); static void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle); + static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList& rgMissionItems, QObject* missionItemParent); // Overrides from PlanElementController void _activeVehicleBeingRemoved(void) final; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 94935caa4aa3171fb41b38589cf9dc39a5ce64a3..85ca185f4f6285bde4d761c5deefb7b2737b454a 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -496,7 +496,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message, boo } MissionItem* item = _missionItems[missionRequest.seq]; - qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq << item->command(); + qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command(); mavlink_message_t messageOut; if (missionItemInt) { diff --git a/src/MissionManager/MissionSettingsComplexItem.cc b/src/MissionManager/MissionSettingsComplexItem.cc index d79290a3105a591e8e0ac437fc5b5774c61aea37..723ad42c44241daf5621af1db4987780e2e43542 100644 --- a/src/MissionManager/MissionSettingsComplexItem.cc +++ b/src/MissionManager/MissionSettingsComplexItem.cc @@ -15,6 +15,7 @@ #include "SimpleMissionItem.h" #include "SettingsManager.h" #include "AppSettings.h" +#include "MissionCommandUIInfo.h" #include @@ -180,7 +181,7 @@ void MissionSettingsComplexItem::appendMissionItems(QList& items, items.append(item); if (_specifyMissionFlightSpeed) { - qDebug() << _missionFlightSpeedFact.rawValue().toDouble(); + qCDebug(MissionSettingsComplexItemLog) << "Appending MAV_CMD_DO_CHANGE_SPEED"; MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_CHANGE_SPEED, MAV_FRAME_MISSION, @@ -198,6 +199,85 @@ void MissionSettingsComplexItem::appendMissionItems(QList& items, _cameraSection.appendMissionItems(items, missionItemParent, seqNum); } +bool MissionSettingsComplexItem::addMissionEndAction(QList& items, int seqNum, QObject* missionItemParent) +{ + MissionItem* item = NULL; + + // IMPORTANT NOTE: If anything changes here you must also change MissionSettingsComplexItem::scanForMissionSettings + + // Find last waypoint coordinate information so we have a lat/lon/alt to use + QGeoCoordinate lastWaypointCoord; + MAV_FRAME lastWaypointFrame; + + bool found = false; + for (int i=items.count()-1; i>0; i--) { + MissionItem* missionItem = items[i]; + + const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, (MAV_CMD)missionItem->command()); + if (uiInfo->specifiesCoordinate() && !uiInfo->isStandaloneCoordinate()) { + lastWaypointCoord = missionItem->coordinate(); + lastWaypointFrame = missionItem->frame(); + found = true; + break; + } + } + if (!found) { + return false; + } + + switch(_missionEndActionFact.rawValue().toInt()) { + case MissionEndLoiter: + qCDebug(MissionSettingsComplexItemLog) << "Appending end action Loiter seqNum" << seqNum; + item = new MissionItem(seqNum, + MAV_CMD_NAV_LOITER_UNLIM, + lastWaypointFrame, + 0, 0, // param 1-2 unused + 0, // use default loiter radius + 0, // param 4 not used + lastWaypointCoord.latitude(), + lastWaypointCoord.longitude(), + lastWaypointCoord.altitude(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); + break; + case MissionEndLand: + qCDebug(MissionSettingsComplexItemLog) << "Appending end action Land seqNum" << seqNum; + item = new MissionItem(seqNum, + MAV_CMD_NAV_LAND, + lastWaypointFrame, + 0, // abort Altitude + 0, 0, // not used + 0, // yaw + lastWaypointCoord.latitude(), + lastWaypointCoord.longitude(), + 0, // altitude + true, // autoContinue + false, // isCurrentItem + missionItemParent); + break; + case MissionEndRTL: + qCDebug(MissionSettingsComplexItemLog) << "Appending end action RTL seqNum" << seqNum; + item = new MissionItem(seqNum, + MAV_CMD_NAV_RETURN_TO_LAUNCH, + MAV_FRAME_MISSION, + 0, 0, 0, 0, 0, 0, 0, // param 1-7 not used + true, // autoContinue + false, // isCurrentItem + missionItemParent); + break; + default: + break; + } + + if (item) { + items.append(item); + return true; + } else { + return false; + } +} + bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle) { bool foundSpeed = false; @@ -245,6 +325,7 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu settingsItem->setSpecifyMissionFlightSpeed(true); settingsItem->missionFlightSpeed()->setRawValue(missionItem.param2()); visualItems->removeAt(scanIndex)->deleteLater(); + qCDebug(MissionSettingsComplexItemLog) << "Scan: Found MAV_CMD_DO_CHANGE_SPEED"; continue; } stopLooking = true; @@ -254,6 +335,7 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu if (!foundCameraSection) { if (settingsItem->_cameraSection.scanForCameraSection(visualItems, scanIndex)) { foundCameraSection = true; + qCDebug(MissionSettingsComplexItemLog) << "Scan: Found Camera Section"; continue; } } @@ -262,6 +344,43 @@ bool MissionSettingsComplexItem::scanForMissionSettings(QmlObjectListModel* visu } } + // Look at the end of the mission for end actions + + int lastIndex = visualItems->count() - 1; + SimpleMissionItem* item = visualItems->value(lastIndex); + if (item) { + MissionItem& missionItem = item->missionItem(); + + switch ((MAV_CMD)item->command()) { + case MAV_CMD_NAV_LOITER_UNLIM: + if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0) { + qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action Loiter"; + settingsItem->missionEndAction()->setRawValue(MissionEndLoiter); + visualItems->removeAt(lastIndex)->deleteLater(); + } + break; + + case MAV_CMD_NAV_LAND: + if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param7() == 0) { + qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action Land"; + settingsItem->missionEndAction()->setRawValue(MissionEndLand); + visualItems->removeAt(lastIndex)->deleteLater(); + } + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) { + qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action RTL"; + settingsItem->missionEndAction()->setRawValue(MissionEndRTL); + visualItems->removeAt(lastIndex)->deleteLater(); + } + break; + + default: + break; + } + } + return foundSpeed || foundCameraSection; } diff --git a/src/MissionManager/MissionSettingsComplexItem.h b/src/MissionManager/MissionSettingsComplexItem.h index 439f845c81734894c330061fcf663291bccd0893..9684610a82c9865856e5cc0091a2197849a91fc2 100644 --- a/src/MissionManager/MissionSettingsComplexItem.h +++ b/src/MissionManager/MissionSettingsComplexItem.h @@ -52,7 +52,14 @@ public: QObject* cameraSection(void) { return &_cameraSection; } /// Scans the loaded items for settings items - static bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicl); + static bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle); + + /// Adds the optional mission end action to the list + /// @param items Mission items list to append to + /// @param seqNum Sequence number for new item + /// @param missionItemParent Parent for newly allocated MissionItems + /// @return true: Mission end action was added + bool addMissionEndAction(QList& items, int seqNum, QObject* missionItemParent); // Overrides from ComplexMissionItem