From 28c0bf9e8bcc4a157f7a1e3a662107d7039809fc Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sat, 9 Jul 2016 12:31:53 -0700
Subject: [PATCH] Copy coordinate frame from prev waypoint as well

---
 src/MissionManager/MissionController.cc | 18 +++++++++++-------
 src/MissionManager/MissionController.h  |  2 +-
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index a4ebe7b5d..cd5383857 100644
--- a/src/MissionManager/MissionController.cc
+++ b/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;
diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h
index bdc10d2ca..05ea06cf1 100644
--- a/src/MissionManager/MissionController.h
+++ b/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);
-- 
2.22.0