From 94fbc66025d4ca4ce9c574a6b8c610844f57b05c Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Sun, 1 Apr 2018 11:24:44 -0700
Subject: [PATCH 1/2] ArduPlane Start Mission support

---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 35 ++++++++++++++++++++++-------
 1 file changed, 27 insertions(+), 8 deletions(-)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 744114b..94c5934 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -951,7 +951,6 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
         return false;
     }
 
-    // FIXME: Is this needed?
     if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
         return false;
@@ -966,13 +965,34 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
     return true;
 }
 
-// FIXME: Review for a better way to do this
 void APMFirmwarePlugin::startMission(Vehicle* vehicle)
 {
-    double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
+    if (vehicle->flying()) {
+        // Vehicle already in the air, we just need to switch to auto
+        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
+            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
+        }
+        return;
+    }
+
+    if (vehicle->fixedWing()) {
+        // Fixed wing will automatically start a mission if you switch to Auto while armed
+        if (!vehicle->armed()) {
+            // First switch to flight mode we can arm from
+            if (!_setFlightModeAndValidate(vehicle, "Guided")) {
+                qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
+                return;
+            }
 
-    if (!vehicle->flying()) {
+            if (!_armVehicleAndValidate(vehicle)) {
+                qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
+                return;
+            }
+        }
+    } else {
+        // Copter will not automatically start a mission from the ground so we have to command it to takeoff first
         if (_guidedModeTakeoff(vehicle, qQNaN())) {
+            double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
 
             // Wait for vehicle to get off ground before switching to auto (10 seconds)
             bool didTakeoff = false;
@@ -989,13 +1009,12 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
                 qgcApp()->showMessage(tr("Unable to start mission. Vehicle takeoff failed."));
                 return;
             }
-        } else {
-            return;
         }
     }
 
-    if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
-        qgcApp()->showMessage(tr("Unable to start mission. Vehicle failed to change to auto."));
+    // Final step is to go into Auto
+    if (!_setFlightModeAndValidate(vehicle, "Auto")) {
+        qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
         return;
     }
 }

From 79b4c38343f57087e4fc73085ad8296a35b71877 Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Sun, 1 Apr 2018 12:15:39 -0700
Subject: [PATCH 2/2] ArduCopter Start Mission change

---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 25 ++++++-------------------
 1 file changed, 6 insertions(+), 19 deletions(-)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index 94c5934..afea0fc 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -990,26 +990,13 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
             }
         }
     } else {
-        // Copter will not automatically start a mission from the ground so we have to command it to takeoff first
-        if (_guidedModeTakeoff(vehicle, qQNaN())) {
-            double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
-
-            // Wait for vehicle to get off ground before switching to auto (10 seconds)
-            bool didTakeoff = false;
-            for (int i=0; i<100; i++) {
-                if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
-                    didTakeoff = true;
-                    break;
-                }
-                QGC::SLEEP::msleep(100);
-                qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
-            }
-
-            if (!didTakeoff) {
-                qgcApp()->showMessage(tr("Unable to start mission. Vehicle takeoff failed."));
-                return;
-            }
+        // Copter will automatically start a mission from the ground if you change to Auto and do a START+MIS
+        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
+            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
+            return;
         }
+
+        vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_MISSION_START, true /*show error */);
     }
 
     // Final step is to go into Auto