Merge pull request #5652 from DonLakeFlyer/GuidedTakeoff

Guided takeoff

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -15,6 +15,7 @@
 #include "APMAirframeComponentController.h"
 #include "APMSensorsComponentController.h"
 #include "MissionManager.h"
+#include "ParameterManager.h"
 
 #include <QTcpSocket>
 
@@ -158,11 +159,18 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
 
 bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
 {
-    Q_UNUSED(vehicle);
-
-    uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
+    uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
+    if (vehicle->multiRotor()) {
+        available |= TakeoffVehicleCapability;
+    } else if (vehicle->fixedWing()) {
+        // Quad plane supports takeoff
+        if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) &&
+            vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) {
+            available |= TakeoffVehicleCapability;
+        }
+    }
 
-    return (capabilities & vehicleCapabilities) == capabilities;
+    return (capabilities & available) == capabilities;
 }
 
 QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
@@ -880,3 +888,88 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
 
     vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
 }
+
+bool APMFirmwarePlugin::isVtol(const Vehicle* vehicle) const
+{
+    if (vehicle->fixedWing()) {
+        if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) &&
+            vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
+{
+    _guidedModeTakeoff(vehicle);
+}
+
+bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
+{
+    QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
+    float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters
+
+    float takeoffAlt = 0;
+    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
+        Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
+        takeoffAlt = takeoffAltFact->rawValue().toDouble();
+    }
+    if (takeoffAlt <= 0) {
+        takeoffAlt = 2.5;
+    } else {
+        takeoffAlt /= paramDivisor;   // centimeters -> meters
+    }
+
+    if (!_setFlightModeAndValidate(vehicle, "Guided")) {
+        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
+        return false;
+    }
+
+    // FIXME: Is this needed?
+    if (!_armVehicleAndValidate(vehicle)) {
+        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
+        return false;
+    }
+
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            MAV_CMD_NAV_TAKEOFF,
+                            true, // show error
+                            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+                            takeoffAlt);
+
+    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)) {
+
+            // 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 (!didTakeoff) {
+                qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
+                return;
+            }
+        }
+    }
+
+    if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
+        qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
+        return;
+    }
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -74,9 +74,12 @@ public:
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
     AutoPilotPlugin*    autopilotPlugin                 (Vehicle* vehicle) final;
-    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
+    bool                isVtol                          (const Vehicle* vehicle) const final;
+    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
     void                setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
+    void                guidedModeTakeoff               (Vehicle* vehicle) final;
     void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
+    void                startMission                    (Vehicle* vehicle) final;
     QStringList         flightModes                     (Vehicle* vehicle) final;
     QString             flightMode                      (uint8_t base_mode, uint32_t custom_mode) const final;
     bool                setFlightMode                   (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
@@ -120,7 +123,8 @@ private:
     bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message);
     void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
     void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
-    void _soloVideoHandshake(Vehicle* vehicle);
+    void _soloVideoHandshake(Vehicle* vehicle);    
+    bool _guidedModeTakeoff(Vehicle* vehicle);
 
     // Any instance data here must be global to all vehicles
     // Vehicle specific data should go into APMFirmwarePluginInstanceData

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -160,45 +160,6 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
     _setFlightModeAndValidate(vehicle, "Land");
 }
 
-void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
-{
-    _guidedModeTakeoff(vehicle);
-}
-
-bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
-{
-    QString takeoffAltParam("PILOT_TKOFF_ALT");
-
-    float takeoffAlt = 0;
-    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
-        Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
-        takeoffAlt = takeoffAltFact->rawValue().toDouble();
-    }
-    if (takeoffAlt <= 0) {
-        takeoffAlt = 2.5;
-    } else {
-        takeoffAlt /= 100;   // centimeters -> meters
-    }
-
-    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 false;
-    }
-
-    vehicle->sendMavCommand(vehicle->defaultComponentId(),
-                            MAV_CMD_NAV_TAKEOFF,
-                            true, // show error
-                            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
-                            takeoffAlt);
-
-    return true;
-}
-
 bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle)
 {
     Q_UNUSED(vehicle);
@@ -223,34 +184,3 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle*
     return true;
 }
 
-void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle)
-{
-    double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
-
-    if (!vehicle->flying()) {
-        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);
-            }
-
-            if (!didTakeoff) {
-                qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
-                return;
-            }
-        }
-    }
-
-    if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
-        qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
-        return;
-    }
-}
-

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h

@@ -57,7 +57,6 @@ public:
 
     // Overrides from FirmwarePlugin
     void    guidedModeLand                      (Vehicle* vehicle) final;
-    void    guidedModeTakeoff                   (Vehicle* vehicle) final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     int     remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
     bool    multiRotorCoaxialMotors             (Vehicle* vehicle) final;
@@ -68,13 +67,10 @@ public:
     QString takeControlFlightMode               (void) const override { return QString("Loiter"); }
     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;
     static FirmwarePlugin::remapParamNameMajorVersionMap_t  _remapParamName;
-
-    bool _guidedModeTakeoff(Vehicle* vehicle);
 };
 
 #endif

src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h

@@ -60,7 +60,6 @@ public:
     QString offlineEditingParamFile                 (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
     QString autoDisarmParameter                     (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); }
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
-    const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -530,3 +530,19 @@ bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitc
     yawSupported = false;
     return false;
 }
+
+bool FirmwarePlugin::isVtol(const Vehicle* vehicle) const
+{
+    switch (vehicle->vehicleType()) {
+    case MAV_TYPE_VTOL_DUOROTOR:
+    case MAV_TYPE_VTOL_QUADROTOR:
+    case MAV_TYPE_VTOL_TILTROTOR:
+    case MAV_TYPE_VTOL_RESERVED2:
+    case MAV_TYPE_VTOL_RESERVED3:
+    case MAV_TYPE_VTOL_RESERVED4:
+    case MAV_TYPE_VTOL_RESERVED5:
+        return true;
+    default:
+        return false;
+    }
+}

src/FirmwarePlugin/FirmwarePlugin.h

@@ -45,6 +45,7 @@ public:
         PauseVehicleCapability =            1 << 2, ///< Vehicle supports pausing at current location
         GuidedModeCapability =              1 << 3, ///< Vehicle supports guided mode commands
         OrbitModeCapability =               1 << 4, ///< Vehicle supports orbit mode
+        TakeoffVehicleCapability =          1 << 5, ///< Vehicle supports guided takeoff
     } FirmwareCapabilities;
 
     /// Maps from on parameter name to another
@@ -286,6 +287,9 @@ public:
     /// @return true: vehicle has gimbal, false: gimbal support unknown
     virtual bool hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported);
 
+    /// Returns true if the vehicle is a VTOL
+    virtual bool isVtol(const Vehicle* vehicle) const;
+
     // FIXME: Hack workaround for non pluginize FollowMe support
     static const char* px4FollowMeFlightMode;
 

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -227,11 +227,12 @@ bool PX4FirmwarePlugin::supportsManualControl(void)
 
 bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
 {
-    if (vehicle->multiRotor()) {
-        return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability /*| OrbitModeCapability still NYI*/)) == capabilities;
-    } else {
-        return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
+    int available = MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
+    if (vehicle->multiRotor() || vehicle->vtol()) {
+        available |= TakeoffVehicleCapability;
     }
+
+    return (capabilities & available) == capabilities;
 }
 
 void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)

src/FlightDisplay/GuidedActionsController.qml

@@ -92,7 +92,7 @@ Item {
     property bool showArm:              _activeVehicle && !_vehicleArmed
     property bool showDisarm:           _activeVehicle && _vehicleArmed && !_vehicleFlying
     property bool showRTL:              _activeVehicle && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode
-    property bool showTakeoff:          _activeVehicle && _activeVehicle.guidedModeSupported && !_vehicleFlying  && !_activeVehicle.fixedWing
+    property bool showTakeoff:          _activeVehicle && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying
     property bool showLand:             _activeVehicle && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode
     property bool showStartMission:     _activeVehicle && _missionAvailable && !_missionActive && !_vehicleFlying
     property bool showContinueMission:  _activeVehicle && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1)

src/Vehicle/Vehicle.cc

@@ -2094,18 +2094,7 @@ bool Vehicle::multiRotor(void) const
 
 bool Vehicle::vtol(void) const
 {
-    switch (vehicleType()) {
-    case MAV_TYPE_VTOL_DUOROTOR:
-    case MAV_TYPE_VTOL_QUADROTOR:
-    case MAV_TYPE_VTOL_TILTROTOR:
-    case MAV_TYPE_VTOL_RESERVED2:
-    case MAV_TYPE_VTOL_RESERVED3:
-    case MAV_TYPE_VTOL_RESERVED4:
-    case MAV_TYPE_VTOL_RESERVED5:
-        return true;
-    default:
-        return false;
-    }
+    return _firmwarePlugin->isVtol(this);
 }
 
 bool Vehicle::supportsManualControl(void) const
@@ -2224,6 +2213,11 @@ bool Vehicle::orbitModeSupported() const
     return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
 }
 
+bool Vehicle::takeoffVehicleSupported() const
+{
+    return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability);
+}
+
 void Vehicle::guidedModeRTL(void)
 {
     if (!guidedModeSupported()) {

src/Vehicle/Vehicle.h

@@ -319,23 +319,14 @@ public:
     Q_PROPERTY(QmlObjectListModel*  adsbVehicles            READ adsbVehicles                                           CONSTANT)
     Q_PROPERTY(bool              initialPlanRequestComplete READ initialPlanRequestComplete                             NOTIFY initialPlanRequestCompleteChanged)
 
-    /// true: Vehicle is flying, false: Vehicle is on ground
-    Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged)
-
-    /// true: Vehicle is flying, false: Vehicle is on ground
-    Q_PROPERTY(bool landing READ landing NOTIFY landingChanged)
-
-    /// true: Vehicle is in Guided mode and can respond to guided commands, false: vehicle cannot respond to direct control commands
-    Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged)
-
-    /// true: Guided mode commands are supported by this vehicle
-    Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT)
-
-    /// true: pauseVehicle command is supported
-    Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT)
-
-    /// true: Orbit mode is supported by this vehicle
-    Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT)
+    // Vehicle state used for guided control
+    Q_PROPERTY(bool flying                  READ flying NOTIFY flyingChanged)                               ///< Vehicle is flying
+    Q_PROPERTY(bool landing                 READ landing NOTIFY landingChanged)                             ///< Vehicle is in landing pattern (DO_LAND_START)
+    Q_PROPERTY(bool guidedMode              READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged)   ///< Vehicle is in Guided mode and can respond to guided commands
+    Q_PROPERTY(bool guidedModeSupported     READ guidedModeSupported CONSTANT)                              ///< Guided mode commands are supported by this vehicle
+    Q_PROPERTY(bool pauseVehicleSupported   READ pauseVehicleSupported CONSTANT)                            ///< Pause vehicle command is supported
+    Q_PROPERTY(bool orbitModeSupported      READ orbitModeSupported CONSTANT)                               ///< Orbit mode is supported by this vehicle
+    Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT)                          ///< Guided takeoff supported
 
     Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT)
 
@@ -441,9 +432,10 @@ public:
     Q_INVOKABLE void motorTest(int motor, int percent, int timeoutSecs);
 #endif
 
-    bool guidedModeSupported(void) const;
-    bool pauseVehicleSupported(void) const;
-    bool orbitModeSupported(void) const;
+    bool guidedModeSupported    (void) const;
+    bool pauseVehicleSupported  (void) const;
+    bool orbitModeSupported     (void) const;
+    bool takeoffVehicleSupported(void) const;
 
     // Property accessors