Merge pull request #4844 from DonLakeFlyer/MissionEndAction

Plan: Support for Mission End Actions

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 {

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<MissionItem*>& rgMissionItems, QObject* missionItemParent)
+{
+    bool endActionSet = false;
+    int lastSeqNum = 0;
+
+    for (int i=0; i<visualMissionItems->count(); i++) {
+        VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(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<MissionSettingsComplexItem*>(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<MissionItem*> missionItems;
+        QList<MissionItem*> rgMissionItems;
 
-        for (int i=0; i<visualMissionItems->count(); i++) {
-            VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(visualMissionItems->get(i));
+        _convertToMissionItems(visualMissionItems, rgMissionItems, vehicle);
 
-            visualItem->appendMissionItems(missionItems, NULL);
-        }
+        vehicle->missionManager()->writeMissionItems(rgMissionItems);
 
-        vehicle->missionManager()->writeMissionItems(missionItems);
-
-        for (int i=0; i<missionItems.count(); i++) {
-            missionItems[i]->deleteLater();
+        for (int i=0; i<rgMissionItems.count(); i++) {
+            rgMissionItems[i]->deleteLater();
         }
     }
 }
@@ -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,30 @@ void MissionController::saveToFile(const QString& filename)
         missionFileObject[_jsonHoverSpeedKey] = _activeVehicle->defaultHoverSpeed();
 
         // Save the visual items
-        QJsonArray  rgMissionItems;
+
+        QJsonArray rgJsonMissionItems;
         for (int i=0; i<_visualItems->count(); i++) {
             VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
-            visualItem->save(rgMissionItems);
+
+            visualItem->save(rgJsonMissionItems);
+        }
+
+        // Mission settings has a special case for end mission action
+        if (settingsItem) {
+            QList<MissionItem*> 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; i<rgMissionItems.count(); i++) {
+                rgMissionItems[i]->deleteLater();
+            }
         }
-        missionFileObject[_jsonItemsKey] = rgMissionItems;
+
+        missionFileObject[_jsonItemsKey] = rgJsonMissionItems;
 
         QJsonDocument saveDoc(missionFileObject);
         file.write(saveDoc.toJson());

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<MissionItem*>& rgMissionItems, QObject* missionItemParent);
 
     // Overrides from PlanElementController
     void _activeVehicleBeingRemoved(void) final;

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) {

src/MissionManager/MissionSettingsComplexItem.cc

@@ -15,6 +15,7 @@
 #include "SimpleMissionItem.h"
 #include "SettingsManager.h"
 #include "AppSettings.h"
+#include "MissionCommandUIInfo.h"
 
 #include <QPolygonF>
 
@@ -180,7 +181,7 @@ void MissionSettingsComplexItem::appendMissionItems(QList<MissionItem*>& 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<MissionItem*>& items,
     _cameraSection.appendMissionItems(items, missionItemParent, seqNum);
 }
 
+bool MissionSettingsComplexItem::addMissionEndAction(QList<MissionItem*>& 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,
+                               MAV_FRAME_GLOBAL_RELATIVE_ALT,
+                               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<SimpleMissionItem*>(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;
 }
 

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<MissionItem*>& items, int seqNum, QObject* missionItemParent);
 
     // Overrides from ComplexMissionItem