diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index e2c6d342faf09fa73df112ae134e5a577ade2386..0b11ea73d1d84bff54f3bd425a942c941a4863c2 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -773,7 +773,7 @@ void ParameterLoader::_saveToEEPROM(void) { if (_saveRequired) { _saveRequired = false; - if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) { + if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) { mavlink_message_t msg; mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index aceb859adbcf1b3c13b3c053379fc719583ba39d..c1b744f20843b8af46c6449a6cd99e9746f3b750 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -143,7 +143,7 @@ APMFirmwarePlugin::APMFirmwarePlugin(void) } -bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities) +bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities) { return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities; } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 357189b7d70dcb062496292de2e45b06dee183af..56f935aa8ea089264a6cb1e39d2b5689d4109a2f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -66,14 +66,14 @@ private: class APMFirmwarePlugin : public FirmwarePlugin { Q_OBJECT - + public: // Overrides from FirmwarePlugin QList componentsForVehicle(AutoPilotPlugin* vehicle) final; QList supportedMissionCommands(void) final; - bool isCapable(FirmwareCapabilities capabilities); + bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities); 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; @@ -103,7 +103,7 @@ protected: private slots: void _artooSocketError(QAbstractSocket::SocketError socketError); - + private: void _adjustSeverity(mavlink_message_t* message) const; void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 070cd30ef17dc035d7f96d2d2bfc8b09abb26faa..6aa3e0542a8a62f2263245ddcbd3c65bea89bb9d 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -98,7 +98,7 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue; } -bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities) +bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities) { return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities; } diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 10c7fae922f95528579338cf49cbfb0d157a7493..394dd10bac1ad294a8a3a249c81ef5a484cf63c6 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -48,12 +48,12 @@ public: class ArduCopterFirmwarePlugin : public APMFirmwarePlugin { Q_OBJECT - + public: ArduCopterFirmwarePlugin(void); // Overrides from FirmwarePlugin - bool isCapable(FirmwareCapabilities capabilities) final; + bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) final; bool isPaused(const Vehicle* vehicle) const final; void setGuidedMode(Vehicle* vehicle, bool guidedMode) final; void pauseVehicle(Vehicle* vehicle) final; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 7e3ffcee1f09ff0268d947a9a227c7204e259b54..60e988928df2e2225fd7414461c6c242e7975d91 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -15,8 +15,9 @@ const char* guided_mode_not_supported_by_vehicle = "Guided mode not supported by Vehicle."; -bool FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) +bool FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) { + Q_UNUSED(vehicle); Q_UNUSED(capabilities); return false; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 72f09b85600a9f16a1f4a93c630d95873bcded68..bf3539e727e9c96c9c760c9c1af968907f4619e4 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -40,7 +40,8 @@ public: SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location - GuidedModeCapability = 1 << 3, ///< Vehicle Support guided mode commands + GuidedModeCapability = 1 << 3, ///< Vehicle Supports guided mode commands + OrbitModeCapability = 1 << 4, ///< Vehicle Supports orbit mode } FirmwareCapabilities; /// Maps from on parameter name to another @@ -62,7 +63,7 @@ public: virtual void initializeVehicle(Vehicle* vehicle); /// @return true: Firmware supports all specified capabilites - virtual bool isCapable(FirmwareCapabilities capabilities); + virtual bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities); /// Returns VehicleComponents for specified Vehicle /// @param vehicle Vehicle to associate with components diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index cf27bb8abee33c8dba7834383e0d5aaddacb3721..92535305d54d06fe43a71d991dd3f4acb47b22ad 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -170,8 +170,11 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void) return 0; // 0 buttons reserved for rc switch simulation } -bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) +bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) { + if(vehicle->multiRotor()) { + return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability | OrbitModeCapability)) == capabilities; + } return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities; } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 23d2804a744c9d3a59e7896edce31a0336cbd069..13d8b420200069f75b5069580ebfb258d39dc979 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -30,7 +30,7 @@ public: QList componentsForVehicle(AutoPilotPlugin* vehicle) final; QList supportedMissionCommands(void) final; - bool isCapable (FirmwareCapabilities capabilities) final; + bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) 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; @@ -53,7 +53,7 @@ public: QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); } void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); } QObject* loadParameterMetaData (const QString& metaDataFile); - bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); + bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message); // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the // names may change. diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml index c8550d84627784664cff23b7552714b2526026b3..cfe307c538b559ff8e55dce7efe8dc7f8dfa6571 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -501,7 +501,7 @@ Item { QGCButton { pointSize: _guidedModeBar._fontPointSize text: qsTr("Orbit") - visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed + visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.orbitModeSupported && _activeVehicle.armed onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit) } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 3663472a84321778b3b58ae1e5e7ab549e07f434..90020ecdf9043c0dbcb8b7aab5b913fbc63500ba 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1223,7 +1223,7 @@ void Vehicle::setArmed(bool armed) bool Vehicle::flightModeSetAvailable(void) { - return _firmwarePlugin->isCapable(FirmwarePlugin::SetFlightModeCapability); + return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability); } QStringList Vehicle::flightModes(void) @@ -1583,12 +1583,17 @@ void Vehicle::setFlying(bool flying) bool Vehicle::guidedModeSupported(void) const { - return _firmwarePlugin->isCapable(FirmwarePlugin::GuidedModeCapability); + return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability); } bool Vehicle::pauseVehicleSupported(void) const { - return _firmwarePlugin->isCapable(FirmwarePlugin::PauseVehicleCapability); + return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability); +} + +bool Vehicle::orbitModeSupported() const +{ + return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability); } void Vehicle::guidedModeRTL(void) @@ -1639,8 +1644,8 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel) void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) { - if (!guidedModeSupported()) { - qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); + if (!orbitModeSupported()) { + qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle.")); return; } _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ad62013a8ed251b95d72b681c4dde498b4970fac..d01d21e5e56d82a45ebc634a625e02f2e13aefc5 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -293,6 +293,9 @@ public: /// 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) + // FactGroup object model properties Q_PROPERTY(Fact* roll READ roll CONSTANT) @@ -380,6 +383,7 @@ public: bool guidedModeSupported(void) const; bool pauseVehicleSupported(void) const; + bool orbitModeSupported(void) const; // Property accessors