Smart RTL support

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)

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;

src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc

@@ -43,7 +43,6 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
     enumToString.insert(GUIDED_NOGPS,   "Guided No GPS");
     enumToString.insert(SAFE_RTL,       "Smart RTL");
 
-
     setEnumToStringMapping(enumToString);
 }
 

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;

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -50,6 +50,7 @@ public:
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const 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;

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);
 }
 

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);

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);
 }
 

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;

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

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()

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();

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; }