Merge pull request #5180 from DonLakeFlyer/ArduPilotStartMission

ArduPilot: Add start mission support

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -186,6 +186,8 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
         takeoffAlt /= 100;   // centimeters -> meters
     }
 
+    setGuidedMode(vehicle, true);
+
     if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
         return;
@@ -285,3 +287,31 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle*
     }
     return true;
 }
+
+void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle)
+{
+    double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
+
+    if (!vehicle->flying()) {
+        guidedModeTakeoff(vehicle);
+
+        // Wait for vehicle to get off ground before switching to auto
+        bool didTakeoff = false;
+        for (int i=0; i<50; i++) {
+            if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
+                didTakeoff = true;
+                break;
+            }
+            QGC::SLEEP::msleep(100);
+            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
+        }
+
+        if (!didTakeoff) {
+            qgcApp()->showMessage(QStringLiteral("Unable to switch to Auto. Vehicle takeoff failed."));
+            return;
+        }
+    }
+
+    vehicle->setFlightMode(missionFlightMode());
+}
+

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -55,27 +55,28 @@ public:
     ArduCopterFirmwarePlugin(void);
 
     // Overrides from FirmwarePlugin
-    bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
-    void setGuidedMode(Vehicle* vehicle, bool guidedMode) final;
-    void pauseVehicle(Vehicle* vehicle) final;
-    void guidedModeRTL(Vehicle* vehicle) final;
-    void guidedModeLand(Vehicle* vehicle) final;
-    void guidedModeTakeoff(Vehicle* vehicle) final;
-    void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
-    void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) final;
+    bool    isCapable                           (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
+    void    setGuidedMode                       (Vehicle* vehicle, bool guidedMode) final;
+    void    pauseVehicle                        (Vehicle* vehicle) final;
+    void    guidedModeRTL                       (Vehicle* vehicle) final;
+    void    guidedModeLand                      (Vehicle* vehicle) final;
+    void    guidedModeTakeoff                   (Vehicle* vehicle) final;
+    void    guidedModeGotoLocation              (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
+    void    guidedModeChangeAltitude            (Vehicle* vehicle, double altitudeChange) final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
-    int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
-    bool multiRotorCoaxialMotors(Vehicle* vehicle) final;
-    bool multiRotorXConfig(Vehicle* vehicle) final;
-    QString geoFenceRadiusParam(Vehicle* vehicle) final;
-    QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
-    QString pauseFlightMode(void) const override { return QString("Brake"); }
-    QString missionFlightMode(void) const override { return QString("Auto"); }
-    QString rtlFlightMode(void) const override { return QString("RTL"); }
-    QString landFlightMode(void) const override { return QString("Land"); }
-    QString takeControlFlightMode(void) const override { return QString("Stablize"); }
-    bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const final;
-    QString autoDisarmParameter(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
+    int     remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
+    bool    multiRotorCoaxialMotors             (Vehicle* vehicle) final;
+    bool    multiRotorXConfig                   (Vehicle* vehicle) final;
+    QString geoFenceRadiusParam                 (Vehicle* vehicle) final;
+    QString offlineEditingParamFile             (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
+    QString pauseFlightMode                     (void) const override { return QString("Brake"); }
+    QString missionFlightMode                   (void) const override { return QString("Auto"); }
+    QString rtlFlightMode                       (void) const override { return QString("RTL"); }
+    QString landFlightMode                      (void) const override { return QString("Land"); }
+    QString takeControlFlightMode               (void) const override { return QString("Stablize"); }
+    bool    vehicleYawsToNextWaypointInMission  (const Vehicle* vehicle) const final;
+    QString autoDisarmParameter                 (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
+    void    startMission                        (Vehicle* vehicle) override;
 
 private:
     static bool _remapParamNameIntialized;