From cec81a7568b65d215db87d33e0a28278f94ad33f Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Wed, 13 Sep 2017 16:34:00 -0700 Subject: [PATCH] Rework guided takeoff to be a capability bit --- src/FirmwarePlugin/FirmwarePlugin.cc | 16 ++++++++++ src/FirmwarePlugin/FirmwarePlugin.h | 4 +++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 9 +++--- src/FlightDisplay/GuidedActionsController.qml | 2 +- src/Vehicle/Vehicle.cc | 18 ++++------- src/Vehicle/Vehicle.h | 32 +++++++------------ 6 files changed, 44 insertions(+), 37 deletions(-) diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 612314f03..f4ae09012 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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; + } +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 65b981e2b..84604746e 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 979855640..c086785f2 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/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) diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index db16ef4de..c079fb675 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/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) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ba290b3ae..c2c4d6c13 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 57e6479a7..bbeb2d248 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 -- 2.22.0