Merge pull request #5188 from DonLakeFlyer/HardenGuided

Better retries on arming, flight mode change and APM guided

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -162,35 +162,42 @@ bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabil
 
 void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
 {
-    vehicle->setFlightMode("RTL");
+    _setFlightModeAndValidate(vehicle, "RTL");
 }
 
 void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 {
-    vehicle->setFlightMode("Land");
+    _setFlightModeAndValidate(vehicle, "Land");
 }
 
 void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
+{
+    _guidedModeTakeoff(vehicle);
+}
+
+bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
 {
     QString takeoffAltParam("PILOT_TKOFF_ALT");
 
-    if (!vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
-        qgcApp()->showMessage(tr("Unable to takeoff, %1 parameter missing.").arg(takeoffAltParam));
-        return;
+    float takeoffAlt = 0;
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
+        Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
+        takeoffAlt = takeoffAltFact->rawValue().toDouble();
     }
-    Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
-    float takeoffAlt = takeoffAltFact->rawValue().toDouble();
     if (takeoffAlt <= 0) {
         takeoffAlt = 2.5;
     } else {
         takeoffAlt /= 100;   // centimeters -> meters
     }
 
-    setGuidedMode(vehicle, true);
+    if (!_setFlightModeAndValidate(vehicle, "Guided")) {
+        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
+        return false;
+    }
 
     if (!_armVehicleAndValidate(vehicle)) {
         qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
-        return;
+        return false;
     }
 
     vehicle->sendMavCommand(vehicle->defaultComponentId(),
@@ -198,6 +205,8 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
                             true, // show error
                             0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
                             takeoffAlt);
+
+    return true;
 }
 
 void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
@@ -246,13 +255,13 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
 
 void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
 {
-    vehicle->setFlightMode("Brake");
+    _setFlightModeAndValidate(vehicle, "Brake");
 }
 
 void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
 {
     if (guidedMode) {
-        vehicle->setFlightMode("Guided");
+        _setFlightModeAndValidate(vehicle, "Guided");
     } else {
         pauseVehicle(vehicle);
     }
@@ -293,25 +302,29 @@ 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;
+        if (_guidedModeTakeoff(vehicle)) {
+
+            // 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);
             }
-            QGC::SLEEP::msleep(100);
-            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
-        }
 
-        if (!didTakeoff) {
-            qgcApp()->showMessage(QStringLiteral("Unable to switch to Auto. Vehicle takeoff failed."));
-            return;
+            if (!didTakeoff) {
+                qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
+                return;
+            }
         }
     }
 
-    vehicle->setFlightMode(missionFlightMode());
+    if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
+        return;
+    }
 }
 

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -81,6 +81,8 @@ public:
 private:
     static bool _remapParamNameIntialized;
     static FirmwarePlugin::remapParamNameMajorVersionMap_t  _remapParamName;
+
+    bool _guidedModeTakeoff(Vehicle* vehicle);
 };
 
 #endif

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -437,21 +437,63 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle)
 
 bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
 {
-    if (!vehicle->armed()) {
+    if (vehicle->armed()) {
+        return true;
+    }
+
+    bool armedChanged = false;
+
+    // We try arming 3 times
+    for (int retries=0; retries<3; retries++) {
         vehicle->setArmed(true);
+
+        // Wait for vehicle to return armed state for 3 seconds
+        for (int i=0; i<30; i++) {
+            if (vehicle->armed()) {
+                armedChanged = true;
+                break;
+            }
+            QGC::SLEEP::msleep(100);
+            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
+        }
+        if (armedChanged) {
+            break;
+        }
     }
 
-    // Wait for vehicle to return armed state for 2 seconds
-    for (int i=0; i<20; i++) {
-        if (vehicle->armed()) {
+    return armedChanged;
+}
+
+bool FirmwarePlugin::_setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode)
+{
+    if (vehicle->flightMode() == flightMode) {
+        return true;
+    }
+
+    bool flightModeChanged = false;
+
+    // We try 3 times
+    for (int retries=0; retries<3; retries++) {
+        vehicle->setFlightMode(flightMode);
+
+        // Wait for vehicle to return flight mode
+        for (int i=0; i<30; i++) {
+            if (vehicle->flightMode() == flightMode) {
+                flightModeChanged = true;
+                break;
+            }
+            QGC::SLEEP::msleep(100);
+            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
+        }
+        if (flightModeChanged) {
             break;
         }
-        QGC::SLEEP::msleep(100);
-        qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
     }
-    return vehicle->armed();
+
+    return flightModeChanged;
 }
 
+
 void FirmwarePlugin::batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, double& hoverAmps, double& cruiseAmps) const
 {
     Q_UNUSED(vehicle);

src/FirmwarePlugin/FirmwarePlugin.h

@@ -296,10 +296,14 @@ public:
     static const char* px4FollowMeFlightMode;
 
 protected:
-    // Arms the vehicle, waiting for the arm state to change.
+    // Arms the vehicle with validation and retries
     // @return: true - vehicle armed, false - vehicle failed to arm
     bool _armVehicleAndValidate(Vehicle* vehicle);
 
+    // Sets the vehicle to the specified flight mode with validation and retries
+    // @return: true - vehicle in specified flight mode, false - flight mode change failed
+    bool _setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode);
+
 private:
     QVariantList _toolBarIndicatorList;
     static QVariantList _cameraList;    ///< Standard QGC camera list

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -331,12 +331,12 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
 
 void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
 {
-    vehicle->setFlightMode(_rtlFlightMode);
+    _setFlightModeAndValidate(vehicle, _rtlFlightMode);
 }
 
 void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
 {
-    vehicle->setFlightMode(_landingFlightMode);
+    _setFlightModeAndValidate(vehicle, _landingFlightMode);
 }
 
 void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
@@ -457,13 +457,13 @@ void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
         return;
     }
 
-    vehicle->setFlightMode(missionFlightMode());
+    _setFlightModeAndValidate(vehicle, missionFlightMode());
 }
 
 void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
 {
     if (guidedMode) {
-        vehicle->setFlightMode(_holdFlightMode);
+        _setFlightModeAndValidate(vehicle, _holdFlightMode);
     } else {
         pauseVehicle(vehicle);
     }