New sequence for PX4 Takeoff

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -139,7 +139,7 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 
 void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
 {
-    if (!_armVehicle(vehicle)) {
+    if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
         return;
     }

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -435,7 +435,7 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle)
     return vehicle->multiRotor() ? false : true;
 }
 
-bool FirmwarePlugin::_armVehicle(Vehicle* vehicle)
+bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
 {
     if (!vehicle->armed()) {
         vehicle->setArmed(true);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -303,7 +303,7 @@ public:
 protected:
     // Arms the vehicle, waiting for the arm state to change.
     // @return: true - vehicle armed, false - vehicle failed to arm
-    bool _armVehicle(Vehicle* vehicle);
+    bool _armVehicleAndValidate(Vehicle* vehicle);
 
 private:
     QVariantList _toolBarIndicatorList;

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -352,6 +352,29 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate&
                             centerCoord.isValid() ? centerCoord.altitude()  : NAN);
 }
 
+void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
+{
+    Q_UNUSED(vehicleId);
+    Q_UNUSED(component);
+    Q_UNUSED(noReponseFromVehicle);
+
+    Vehicle* vehicle = dynamic_cast<Vehicle*>(sender());
+    if (!vehicle) {
+        qWarning() << "Dynamic cast failed!";
+        return;
+    }
+
+    if (command == MAV_CMD_NAV_TAKEOFF && result == MAV_RESULT_ACCEPTED) {
+        // Now that we are in takeoff mode we can arm the vehicle which will cause it to takeoff.
+        // We specifically don't retry arming if it fails. This way we don't fight with the user if
+        // They are trying to disarm.
+        disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
+        if (!vehicle->armed()) {
+            vehicle->setArmed(true);
+        }
+    }
+}
+
 void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
 {
     QString takeoffAltParam("MIS_TAKEOFF_ALT");
@@ -367,11 +390,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
     }
     Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
 
-    if (!_armVehicle(vehicle)) {
-        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
-        return;
-    }
-
+    connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
     vehicle->sendMavCommand(vehicle->defaultComponentId(),
                             MAV_CMD_NAV_TAKEOFF,
                             true,                           // show error is fails
@@ -436,7 +455,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
 
 void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
 {
-    if (!_armVehicle(vehicle)) {
+    if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
         return;
     }

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -103,6 +103,9 @@ protected:
     QString _followMeFlightMode;
     QString _simpleFlightMode;
 
+private slots:
+    void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
+
 private:
     void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);