Merge pull request #6293 from DonLakeFlyer/ArduPlaneStartMission

ArduPlane Start Mission support

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,36 +965,43 @@ 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()) {
-        if (_guidedModeTakeoff(vehicle, qQNaN())) {
+    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;
+    }
 
-            // 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 (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 (!didTakeoff) {
-                qgcApp()->showMessage(tr("Unable to start mission. Vehicle takeoff failed."));
+            if (!_armVehicleAndValidate(vehicle)) {
+                qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
                 return;
             }
-        } else {
+        }
+    } else {
+        // 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 */);
     }
 
-    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;
     }
 }