Merge pull request #5415 from mavlink/pr-fix-mode-switching

Fix mode switching

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -485,7 +485,7 @@ bool FirmwarePlugin::_setFlightModeAndValidate(Vehicle* vehicle, const QString&
         vehicle->setFlightMode(flightMode);
 
         // Wait for vehicle to return flight mode
-        for (int i=0; i<30; i++) {
+        for (int i=0; i<22; i++) {
             if (vehicle->flightMode() == flightMode) {
                 flightModeChanged = true;
                 break;

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -452,12 +452,15 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
 
 void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
 {
-    if (!_armVehicleAndValidate(vehicle)) {
-        qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
-        return;
-    }
 
-    _setFlightModeAndValidate(vehicle, missionFlightMode());
+    if (_setFlightModeAndValidate(vehicle, missionFlightMode())) {
+        if (!_armVehicleAndValidate(vehicle)) {
+            qgcApp()->showMessage(tr("Unable to start mission: Vehicle rejected arming."));
+            return;
+        }
+    } else {
+        qgcApp()->showMessage(tr("Unable to start mission: Vehicle not ready."));
+    }
 }
 
 void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)

src/Vehicle/Vehicle.cc

@@ -789,7 +789,7 @@ void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
 
 void Vehicle::_handleCommandAck(mavlink_message_t& message)
 {
-    bool showError = true;
+    bool showError = false;
 
     mavlink_command_ack_t ack;
     mavlink_msg_command_ack_decode(&message, &ack);