diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 76fcda2bbae8b3c754c5a6f0715fbed5d25d19ee..ffa0cb7469f86806cd1eb677a77776cd321a3e2c 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -897,9 +897,9 @@ void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */); } -void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { - _setFlightModeAndValidate(vehicle, rtlFlightMode()); + _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode()); } void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index ddf754ed54ef60e4a2edc956c5a3e53b0a6ca31c..b7cbfbb94ad7cf08732fc4b158f62fbd4548b749 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -86,9 +86,10 @@ public: bool isGuidedMode (const Vehicle* vehicle) const override; QString gotoFlightMode (void) const override { return QStringLiteral("Guided"); } QString rtlFlightMode (void) const override { return QString("RTL"); } + QString smartRTLFlightMode (void) const override { return QString("Smart RTL"); } QString missionFlightMode (void) const override { return QString("Auto"); } void pauseVehicle (Vehicle* vehicle) override; - void guidedModeRTL (Vehicle* vehicle) override; + void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) override; bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override; void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) override; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 93d653b9ce7171bc7fbecfcd4f5106725c022fc6..dce148791d7e5bf2de149e0914b815dafd15a581 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -23,26 +23,25 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : APMCustomMode(mode, settable) { QMap enumToString; - enumToString.insert(STABILIZE, "Stabilize"); - enumToString.insert(ACRO, "Acro"); - enumToString.insert(ALT_HOLD, "Altitude Hold"); - enumToString.insert(AUTO, "Auto"); - enumToString.insert(GUIDED, "Guided"); - enumToString.insert(LOITER, "Loiter"); - enumToString.insert(RTL, "RTL"); - enumToString.insert(CIRCLE, "Circle"); - enumToString.insert(LAND, "Land"); - enumToString.insert(DRIFT, "Drift"); - enumToString.insert(SPORT, "Sport"); - enumToString.insert(FLIP, "Flip"); - enumToString.insert(AUTOTUNE, "Autotune"); - enumToString.insert(POS_HOLD, "Position Hold"); - enumToString.insert(BRAKE, "Brake"); - enumToString.insert(THROW, "Throw"); - enumToString.insert(AVOID_ADSB,"Avoid ADSB"); - enumToString.insert(GUIDED_NOGPS,"Guided No GPS"); - enumToString.insert(SAFE_RTL,"Smart RTL"); - + enumToString.insert(STABILIZE, "Stabilize"); + enumToString.insert(ACRO, "Acro"); + enumToString.insert(ALT_HOLD, "Altitude Hold"); + enumToString.insert(AUTO, "Auto"); + enumToString.insert(GUIDED, "Guided"); + enumToString.insert(LOITER, "Loiter"); + enumToString.insert(RTL, "RTL"); + enumToString.insert(CIRCLE, "Circle"); + enumToString.insert(LAND, "Land"); + enumToString.insert(DRIFT, "Drift"); + enumToString.insert(SPORT, "Sport"); + enumToString.insert(FLIP, "Flip"); + enumToString.insert(AUTOTUNE, "Autotune"); + enumToString.insert(POS_HOLD, "Position Hold"); + enumToString.insert(BRAKE, "Brake"); + enumToString.insert(THROW, "Throw"); + enumToString.insert(AVOID_ADSB, "Avoid ADSB"); + enumToString.insert(GUIDED_NOGPS, "Guided No GPS"); + enumToString.insert(SAFE_RTL, "Smart RTL"); setEnumToStringMapping(enumToString); } diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 6bdc9f61910d1cb00d92052da1d72fc9e5f777b0..563baf7b68fd7db0c300ecfbf24c94257cd574a2 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -65,8 +65,9 @@ public: QString pauseFlightMode (void) const override { return QString("Brake"); } QString landFlightMode (void) const override { return QString("Land"); } 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"); } + bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; + QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } + bool supportsSmartRTL (void) const override { return true; } private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index 98d932ec329a78cec63d0bf647b0b78037f8aab8..ae437dec461df7dbf4108d122ba0b094b8e2d927 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -48,8 +48,9 @@ public: QString pauseFlightMode (void) const override { return QString("Hold"); } void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final; int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final; - const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } - bool supportsNegativeThrust(void) final; + const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; } + bool supportsNegativeThrust (void) final; + bool supportsSmartRTL (void) const override { return true; } private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 1790a4a5018274cbb5ae5ae7863f42f6965ac720..00c4076ecec69ab6db504d10f10964654709da09 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -240,10 +240,11 @@ void FirmwarePlugin::pauseVehicle(Vehicle* vehicle) qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } -void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { // Not supported by generic vehicle Q_UNUSED(vehicle); + Q_UNUSED(smartRTL); qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index df1ebbab7427b548075339d0532cfc22deca5ff3..3fb7ca8ad40241d8c2ae1992cc234af1e7441b71 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -105,6 +105,11 @@ public: /// Returns the flight mode for RTL virtual QString rtlFlightMode(void) const { return QString(); } + /// Returns the flight mode for Smart RTL + virtual QString smartRTLFlightMode(void) const { return QString(); } + + virtual bool supportsSmartRTL(void) const { return false; } + /// Returns the flight mode for Land virtual QString landFlightMode(void) const { return QString(); } @@ -125,7 +130,7 @@ public: virtual void pauseVehicle(Vehicle* vehicle); /// Command vehicle to return to launch - virtual void guidedModeRTL(Vehicle* vehicle); + virtual void guidedModeRTL(Vehicle* vehicle, bool smartRTL); /// Command vehicle to land at current location virtual void guidedModeLand(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 5373488a22f00b9020e3fe6b5d0cf61204b81649..63b7054fff104f56ed5a8077e74866f3f9cf52f2 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -351,8 +351,9 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) NAN); } -void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL) { + Q_UNUSED(smartRTL); _setFlightModeAndValidate(vehicle, _rtlFlightMode); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index edee88893d0ae7788c5f846f6620aacd41cbe152..3e7f794ce770d707fc138e1d2e9ab01c8b83f443 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -44,7 +44,7 @@ public: QString takeControlFlightMode (void) const override { return _manualFlightMode; } QString gotoFlightMode (void) const override { return _holdFlightMode; } void pauseVehicle (Vehicle* vehicle) override; - void guidedModeRTL (Vehicle* vehicle) override; + void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override; void guidedModeLand (Vehicle* vehicle) override; void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override; void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override; diff --git a/src/FlightDisplay/GuidedActionConfirm.qml b/src/FlightDisplay/GuidedActionConfirm.qml index 177e1944ad82a013cb151b143173a44f30027e75..dcb74cae493e09141d792629e2a4f692a175b25d 100644 --- a/src/FlightDisplay/GuidedActionConfirm.qml +++ b/src/FlightDisplay/GuidedActionConfirm.qml @@ -35,6 +35,8 @@ Rectangle { property var actionData property bool hideTrigger: false property var mapIndicator + property alias optionText: optionCheckBox.text + property alias optionChecked: optionCheckBox.checked property real _margins: ScreenTools.defaultFontPixelWidth property bool _emergencyAction: action === guidedController.actionEmergencyStop @@ -101,6 +103,13 @@ Rectangle { wrapMode: Text.WordWrap } + QGCCheckBox { + id: optionCheckBox + anchors.horizontalCenter: parent.horizontalCenter + text: "" + visible: text !== "" + } + // Action confirmation control SliderSwitch { id: slider @@ -115,7 +124,7 @@ Rectangle { altitudeSlider.visible = false } hideTrigger = false - guidedController.executeAction(_root.action, _root.actionData, altitudeChange) + guidedController.executeAction(_root.action, _root.actionData, altitudeChange, _root.optionChecked) if (mapIndicator) { mapIndicator.actionConfirmed() mapIndicator = undefined diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index c099ed669c21dbabb532d645bce9cd9e7bf2e303..953ab6e51fc6559e9e1aa780852a7f387f8f18f8 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -203,7 +203,7 @@ Item { on_FlightModeChanged: { _vehiclePaused = _activeVehicle ? _flightMode === _activeVehicle.pauseFlightMode : false - _vehicleInRTLMode = _activeVehicle ? _flightMode === _activeVehicle.rtlFlightMode : false + _vehicleInRTLMode = _activeVehicle ? _flightMode === _activeVehicle.rtlFlightMode || _flightMode === _activeVehicle.smartRTLFlightMode : false _vehicleInLandMode = _activeVehicle ? _flightMode === _activeVehicle.landFlightMode : false _vehicleInMissionMode = _activeVehicle ? _flightMode === _activeVehicle.missionFlightMode : false // Must be last to get correct signalling for showStartMission popups } @@ -216,6 +216,7 @@ Item { confirmDialog.actionData = actionData confirmDialog.hideTrigger = true confirmDialog.mapIndicator = mapIndicator + confirmDialog.optionText = "" _actionData = actionData switch (actionCode) { case actionArm: @@ -279,6 +280,10 @@ Item { case actionRTL: confirmDialog.title = rtlTitle confirmDialog.message = rtlMessage + if (_activeVehicle.supportsSmartRTL) { + confirmDialog.optionText = qsTr("Smart RTL") + confirmDialog.optionChecked = false + } confirmDialog.hideTrigger = Qt.binding(function() { return !showRTL }) break; case actionChangeAlt: @@ -339,12 +344,12 @@ Item { } // Executes the specified action - function executeAction(actionCode, actionData, actionAltitudeChange) { + function executeAction(actionCode, actionData, actionAltitudeChange, optionChecked) { var i; var rgVehicle; switch (actionCode) { case actionRTL: - _activeVehicle.guidedModeRTL() + _activeVehicle.guidedModeRTL(optionChecked) break case actionLand: _activeVehicle.guidedModeLand() diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 08c3c268f393526426144402928c1313a0d7f6eb..2a129bef6967e95021a60686490a2dbf414286d2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2979,13 +2979,13 @@ QString Vehicle::gotoFlightMode() const return _firmwarePlugin->gotoFlightMode(); } -void Vehicle::guidedModeRTL(void) +void Vehicle::guidedModeRTL(bool smartRTL) { if (!guidedModeSupported()) { qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); return; } - _firmwarePlugin->guidedModeRTL(this); + _firmwarePlugin->guidedModeRTL(this, smartRTL); } void Vehicle::guidedModeLand(void) @@ -3606,6 +3606,16 @@ QString Vehicle::rtlFlightMode(void) const return _firmwarePlugin->rtlFlightMode(); } +QString Vehicle::smartRTLFlightMode(void) const +{ + return _firmwarePlugin->smartRTLFlightMode(); +} + +bool Vehicle::supportsSmartRTL(void) const +{ + return _firmwarePlugin->supportsSmartRTL(); +} + QString Vehicle::landFlightMode(void) const { return _firmwarePlugin->landFlightMode(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 530a4ed13ae9e1aae3959bdedf9967070bc271ba..347097bf48f67fb6c8d939f4d9a17ab6a366daae 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -600,6 +600,8 @@ public: Q_PROPERTY(QString missionFlightMode READ missionFlightMode CONSTANT) Q_PROPERTY(QString pauseFlightMode READ pauseFlightMode CONSTANT) Q_PROPERTY(QString rtlFlightMode READ rtlFlightMode CONSTANT) + Q_PROPERTY(QString smartRTLFlightMode READ smartRTLFlightMode CONSTANT) + Q_PROPERTY(bool supportsSmartRTL READ supportsSmartRTL CONSTANT) Q_PROPERTY(QString landFlightMode READ landFlightMode CONSTANT) Q_PROPERTY(QString takeControlFlightMode READ takeControlFlightMode CONSTANT) Q_PROPERTY(QString firmwareTypeString READ firmwareTypeString NOTIFY firmwareTypeChanged) @@ -698,7 +700,7 @@ public: Q_INVOKABLE void disconnectInactiveVehicle(void); /// Command vehicle to return to launch - Q_INVOKABLE void guidedModeRTL(void); + Q_INVOKABLE void guidedModeRTL(bool smartRTL); /// Command vehicle to land at current location Q_INVOKABLE void guidedModeLand(void); @@ -919,6 +921,8 @@ public: QString missionFlightMode () const; QString pauseFlightMode () const; QString rtlFlightMode () const; + QString smartRTLFlightMode () const; + bool supportsSmartRTL () const; QString landFlightMode () const; QString takeControlFlightMode () const; double defaultCruiseSpeed () const { return _defaultCruiseSpeed; }