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 6338280b71e63bfd210a1bda08a3521d39d55fd9..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; } @@ -183,6 +184,12 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } +void FirmwarePlugin::guidedModeOrbit(Vehicle* /*vehicle*/, const QGeoCoordinate& /*centerCoord*/, double /*radius*/, double /*velocity*/, double /*altitude*/) +{ + // Not supported by generic vehicle + qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); +} + void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) { // Not supported by generic vehicle diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index c64cbdc98ae7af9c35b9d7a7510022ca187af62d..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 @@ -57,30 +58,30 @@ public: /// key: firmware major version /// value: remapParamNameMinorVersionRemapMap_t entry typedef QMap remapParamNameMajorVersionMap_t; - + /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. 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 /// @return List of VehicleComponents for the specified vehicle. Caller owns returned objects and must /// free when no longer needed. virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); - + /// Returns the list of available flight modes virtual QStringList flightModes(Vehicle* vehicle) { - Q_UNUSED(vehicle); - return QStringList(); - } - + Q_UNUSED(vehicle); + return QStringList(); + } + /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable. /// @param base_mode Base mode from mavlink HEARTBEAT message /// @param custom_mode Custom mode from mavlink HEARTBEAT message virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode) const; - + /// Sets base_mode and custom_mode to specified flight mode. /// @param[out] base_mode Base mode for SET_MODE mavlink message /// @param[out] custom_mode Custom mode for SET_MODE mavlink message @@ -109,6 +110,10 @@ public: /// @param altitudeRel Relative altitude to takeoff to virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel); + /// Command vehicle to orbit given center point + /// @param centerCoord Center Coordinates + virtual void guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude); + /// Command vehicle to move to specified location (altitude is included and relative) virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord); @@ -122,7 +127,7 @@ public: /// The remainder can be assigned to Vehicle actions. /// @return -1: reserver all buttons, >0 number of buttons to reserve virtual int manualControlReservedButtonCount(void); - + /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin /// can adjust any message characteristics. This is handy to adjust or differences in mavlink /// spec implementations such that the base code can remain mavlink generic. @@ -130,7 +135,7 @@ public: /// @param message[in,out] Mavlink message to adjust if needed. /// @return false: skip message, true: process message virtual bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); - + /// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics. /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain /// mavlink generic. @@ -145,7 +150,7 @@ public: /// it, it may or may not return a home position back in position 0. /// false: Do not send first item to vehicle, sequence numbers must be adjusted virtual bool sendHomePositionToVehicle(void); - + /// Returns the parameter that is used to identify the default component virtual QString getDefaultComponentIdParam(void) const { return QString(); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 9834ba33747913d18c9b7f8523be60e0cc36ea49..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; } @@ -269,6 +272,29 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle) vehicle->setFlightMode(landingFlightMode); } +void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) +{ + //-- If not in "guided" mode, make it so. + if(!isGuidedMode(vehicle)) + setGuidedMode(vehicle, true); + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_command_long_t cmd; + cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE; + cmd.confirmation = 0; + cmd.param1 = radius; + cmd.param2 = velocity; + cmd.param3 = altitude; + cmd.param4 = NAN; + cmd.param5 = centerCoord.isValid() ? centerCoord.latitude() : NAN; + cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN; + cmd.param7 = centerCoord.isValid() ? centerCoord.altitude() : NAN; + cmd.target_system = vehicle->id(); + cmd.target_component = vehicle->defaultComponentId(); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + vehicle->sendMessageOnPriorityLink(msg); +} + void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) { Q_UNUSED(altitudeRel); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 6a712f96a82fa9434493e27edee5ee07c391cf7c..13d8b420200069f75b5069580ebfb258d39dc979 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -30,18 +30,19 @@ 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; - void setGuidedMode(Vehicle* vehicle, bool guidedMode) final; - void pauseVehicle(Vehicle* vehicle) final; - void guidedModeRTL(Vehicle* vehicle) final; - void guidedModeLand(Vehicle* vehicle) final; - void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final; - void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; - void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; - bool isGuidedMode(const Vehicle* vehicle) const; + void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; + void pauseVehicle (Vehicle* vehicle) final; + void guidedModeRTL (Vehicle* vehicle) final; + void guidedModeLand (Vehicle* vehicle) final; + void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) final; + void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final; + void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; + void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) final; + bool isGuidedMode (const Vehicle* vehicle) const; int manualControlReservedButtonCount(void) final; void initializeVehicle (Vehicle* vehicle) final; bool sendHomePositionToVehicle (void) final; @@ -52,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 cfea5e63e2dc2590ad5b30b2cd19e09560d1d16c..cfe307c538b559ff8e55dce7efe8dc7f8dfa6571 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -279,10 +279,9 @@ Item { anchors.horizontalCenter: parent.horizontalCenter width: guidedModeColumn.width + (_margins * 2) height: guidedModeColumn.height + (_margins * 2) - radius: _margins - color: _lightWidgetBorders ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark + radius: ScreenTools.defaultFontPixelHeight * 0.25 + color: _lightWidgetBorders ? Qt.rgba(qgcPal.mapWidgetBorderLight.r, qgcPal.mapWidgetBorderLight.g, qgcPal.mapWidgetBorderLight.b, 0.8) : Qt.rgba(qgcPal.mapWidgetBorderDark.r, qgcPal.mapWidgetBorderDark.g, qgcPal.mapWidgetBorderDark.b, 0.75) visible: _activeVehicle - opacity: 0.9 z: QGroundControl.zOrderWidgets state: "Shown" @@ -340,6 +339,7 @@ Item { readonly property int confirmChangeAlt: 7 readonly property int confirmGoTo: 8 readonly property int confirmRetask: 9 + readonly property int confirmOrbit: 10 property int confirmActionCode property real _showMargin: _margins @@ -381,6 +381,12 @@ Item { case confirmRetask: _activeVehicle.setCurrentMissionSequence(_flightMap._retaskSequence) break; + case confirmOrbit: + //-- All parameters controlled by RC + _activeVehicle.guidedModeOrbit() + //-- Center on current flight map position and orbit with a 50m radius (velocity/direction controlled by the RC) + //_activeVehicle.guidedModeOrbit(QGroundControl.flightMapPosition, 50.0) + break; default: console.warn(qsTr("Internal error: unknown confirmActionCode"), confirmActionCode) } @@ -429,6 +435,9 @@ Item { case confirmRetask: guidedModeConfirm.confirmText = qsTr("active waypoint change") break; + case confirmOrbit: + guidedModeConfirm.confirmText = qsTr("enter orbit mode") + break; } _guidedModeBar.visible = false guidedModeConfirm.visible = true @@ -488,6 +497,14 @@ Item { visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt) } + + QGCButton { + pointSize: _guidedModeBar._fontPointSize + text: qsTr("Orbit") + visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.orbitModeSupported && _activeVehicle.armed + onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit) + } + } // Row } // Column } // Rectangle - Guided mode buttons diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 230fb67b73f446fa11d9c7757afab2792fcd27fb..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) @@ -1637,6 +1642,15 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel) _firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel); } +void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) +{ + if (!orbitModeSupported()) { + qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle.")); + return; + } + _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude); +} + void Vehicle::pauseVehicle(void) { if (!pauseVehicleSupported()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 07a8f1bb118e5f744f680d5406d2430ede302350..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) @@ -349,6 +352,13 @@ public: /// Command vehicle to change to the specified relatice altitude Q_INVOKABLE void guidedModeChangeAltitude(double altitudeRel); + /// Command vehicle to orbit given center point + /// @param centerCoord Center Coordinates + /// @param radius Distance from vehicle to centerCoord + /// @param velocity Orbit velocity (positive CW, negative CCW) + /// @param altitude Desired Vehicle Altitude + Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN); + /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left /// in guided mode after pause. Q_INVOKABLE void pauseVehicle(void); @@ -373,6 +383,7 @@ public: bool guidedModeSupported(void) const; bool pauseVehicleSupported(void) const; + bool orbitModeSupported(void) const; // Property accessors