Merge pull request #3758 from DonLakeFlyer/CopyCoordFrame

Plan: Copy coordinate frame from prev waypoint as well

src/MissionManager/MissionController.cc

@@ -175,11 +175,13 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
     newItem->setDefaultsForCommand();
     if ((MAV_CMD)newItem->command() == MAV_CMD_NAV_WAYPOINT) {
         double lastValue;
+        MAV_FRAME lastFrame;
 
         if (_findLastAcceptanceRadius(&lastValue)) {
             newItem->missionItem().setParam2(lastValue);
         }
-        if (_findLastAltitude(&lastValue)) {
+        if (_findLastAltitude(&lastValue, &lastFrame)) {
+            newItem->missionItem().setFrame(lastFrame);
             newItem->missionItem().setParam7(lastValue);
         }
     }
@@ -1047,23 +1049,24 @@ void MissionController::_inProgressChanged(bool inProgress)
     }
 }
 
-bool MissionController::_findLastAltitude(double* lastAltitude)
+bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame)
 {
-    bool found = false;
+    bool found = false;    
     double foundAltitude;
+    MAV_FRAME foundFrame;
 
     // Don't use home position
     for (int i=1; i<_visualItems->count(); i++) {
         VisualMissionItem* visualItem = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
 
         if (visualItem->specifiesCoordinate() && !visualItem->isStandaloneCoordinate()) {
-            foundAltitude = visualItem->exitCoordinate().altitude();
-            found = true;
 
             if (visualItem->isSimpleItem()) {
                 SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
-                if ((MAV_CMD)simpleItem->command() == MAV_CMD_NAV_TAKEOFF) {
-                    found = false;
+                if ((MAV_CMD)simpleItem->command() != MAV_CMD_NAV_TAKEOFF) {
+                    foundAltitude = simpleItem->exitCoordinate().altitude();
+                    foundFrame = simpleItem->missionItem().frame();
+                    found = true;
                 }
             }
         }
@@ -1071,6 +1074,7 @@ bool MissionController::_findLastAltitude(double* lastAltitude)
 
     if (found) {
         *lastAltitude = foundAltitude;
+        *frame = foundFrame;
     }
 
     return found;

src/MissionManager/MissionController.h

@@ -103,7 +103,7 @@ private:
     void _autoSyncSend(void);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
     static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
-    bool _findLastAltitude(double* lastAltitude);
+    bool _findLastAltitude(double* lastAltitude, MAV_FRAME* frame);
     bool _findLastAcceptanceRadius(double* lastAcceptanceRadius);
     void _addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter);
     double _normalizeLat(double lat);