smart rtl and smart rtl low battery added

src/FlightDisplay/FlightDisplayView.qml

@@ -119,6 +119,14 @@ QGCView {
         Component.onCompleted: {
             wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer })
         }
+
+        onUploadAndExecuteConfirmRequired: {
+            _guidedController.confirmAction(_guidedController.actionUploadAndExecute)
+        }
+
+        onReturnBatteryLowConfirmRequired: {
+            _guidedController.confirmAction(_guidedController.actionReturnBatteryLow)
+        }
     }
 
     BuiltInPreFlightCheckModel {
@@ -549,6 +557,12 @@ QGCView {
 
             wimaController: wimaController
             planMasterController: masterController
+
+            onInitSmartRTL: {
+                if (wimaController.checkSmartRTLPreCondition()) {
+                    _guidedController.confirmAction(_guidedController.actionInitSmartRTL)
+                }
+            }
         }
 
         //-------------------------------------------------------------------------

src/FlightDisplay/FlightDisplayWimaMenu.qml

@@ -28,6 +28,8 @@ Item {
     property bool   _controllerValid:           planMasterController !== undefined
     property real   _controllerProgressPct:     _controllerValid ? planMasterController.missionController.progressPct : 0
 
+    signal initSmartRTL();
+
     // Progress bar visibility
     on_ControllerProgressPctChanged: {
         if (_controllerProgressPct === 1) {
@@ -213,6 +215,16 @@ Item {
                     Layout.fillWidth:   true
                 }
 
+                QGCButton {
+                    id: buttonSmartRTL
+                    text:               qsTr("Smart RTL")
+                    onClicked:          initSmartRTL()
+                    Layout.fillWidth:   true
+                }
+                // placeholder, remove if you add stuff
+                QGCLabel { text: "" }
+
+
                 // progess bar
                 Rectangle {
                     id:             progressBar

src/FlightDisplay/GuidedActionsController.qml

@@ -53,6 +53,9 @@ Item {
     readonly property string gotoTitle:                     qsTr("Goto Location")
     readonly property string vtolTransitionTitle:           qsTr("VTOL Transition")    
     readonly property string overrideUploadTitle:           qsTr("Override Lock")
+    readonly property string initSmartRTLTitle:             qsTr("Smart RTL")
+    readonly property string uploadAndExecuteTitle:         qsTr("Upload and Execute")
+    readonly property string returnBatteryLowTitle:         qsTr("Battery Low")
 
     readonly property string armMessage:                        qsTr("Arm the vehicle.")
     readonly property string disarmMessage:                     qsTr("Disarm the vehicle")
@@ -72,7 +75,11 @@ Item {
     readonly property string mvPauseMessage:                    qsTr("Pause all vehicles at their current position.")
     readonly property string vtolTransitionFwdMessage:          qsTr("Transition VTOL to fixed wing flight.")
     readonly property string vtolTransitionMRMessage:           qsTr("Transition VTOL to multi-rotor flight.")
-    readonly property string overrideUploadMessage:             qsTr("Vehicle is not inside service area. Upload nevertheless?")
+    readonly property string overrideUploadMessage:             qsTr("Vehicle is not inside service area. Upload nevertheless?")    
+    readonly property string initSmartRTLMessage:               qsTr("Pause the vehicle at it's current position and calculate a return path?")
+    readonly property string uploadAndExecuteMessage:           qsTr("Upload and execute the displayed return path?")
+    readonly property string returnBatteryLowMessage:           qsTr("Upload and execute the displayed return path?")
+
 
     readonly property int actionRTL:                        1
     readonly property int actionLand:                       2
@@ -95,7 +102,10 @@ Item {
     readonly property int actionMVStartMission:             19
     readonly property int actionVtolTransitionToFwdFlight:  20
     readonly property int actionVtolTransitionToMRFlight:   21
-    readonly property int actionOverrideUpload:             22
+    readonly property int actionOverrideUpload:             22    
+    readonly property int actionInitSmartRTL:               23
+    readonly property int actionUploadAndExecute:           24
+    readonly property int actionReturnBatteryLow:           25 // very similar to actionUploadAndExecute, is triggered on low battery
 
     property bool showEmergenyStop:     _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying
     property bool showArm:              _guidedActionsEnabled && !_vehicleArmed
@@ -340,6 +350,21 @@ Item {
             confirmDialog.message = overrideUploadMessage
             confirmDialog.hideTrigger = true
             break
+        case actionInitSmartRTL:
+            confirmDialog.title = initSmartRTLTitle
+            confirmDialog.message = initSmartRTLMessage
+            confirmDialog.hideTrigger = true
+            break
+        case actionUploadAndExecute:
+            confirmDialog.title = uploadAndExecuteTitle
+            confirmDialog.message = uploadAndExecuteMessage
+            confirmDialog.hideTrigger = true
+            break
+        case actionReturnBatteryLow:
+            confirmDialog.title = returnBatteryLowTitle
+            confirmDialog.message = returnBatteryLowMessage
+            confirmDialog.hideTrigger = true
+            break
         default:
             console.warn("Unknown actionCode", actionCode)
             return
@@ -418,6 +443,18 @@ Item {
         case actionOverrideUpload:
             wimaController.forceUploadToVehicle()
             break
+        case actionInitSmartRTL:
+            _activeVehicle.pauseVehicle()
+            wimaController.calcReturnPath()
+            break
+        case actionUploadAndExecute:
+            wimaController.forceUploadToVehicle()
+            _activeVehicle.startMission()
+            break
+        case actionReturnBatteryLow:
+            wimaController.forceUploadToVehicle()
+            _activeVehicle.startMission()
+            break
         default:
             console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
             break

src/Wima/WimaController.h

@@ -56,6 +56,7 @@ public:
     Q_PROPERTY(bool                     uploadOverrideRequired  READ uploadOverrideRequired WRITE setUploadOverrideRequired NOTIFY uploadOverrideRequiredChanged)
     Q_PROPERTY(double                   phaseDistance           READ phaseDistance                                          NOTIFY phaseDistanceChanged)
     Q_PROPERTY(double                   phaseDuration           READ phaseDuration                                          NOTIFY phaseDurationChanged)
+    Q_PROPERTY(bool                     vehicleHasLowBattery    READ vehicleHasLowBattery                                   NOTIFY vehicleHasLowBatteryChanged)
 
 
 
@@ -84,6 +85,7 @@ public:
     bool                        uploadOverrideRequired (void) const;
     double                      phaseDistance          (void) const;
     double                      phaseDuration          (void) const;
+    bool                        vehicleHasLowBattery   (void) const;
 
 
     // Property setters
@@ -98,7 +100,10 @@ public:
     Q_INVOKABLE void resetPhase();
     Q_INVOKABLE bool uploadToVehicle();
     Q_INVOKABLE bool forceUploadToVehicle();
-    Q_INVOKABLE void removeFromVehicle();
+    Q_INVOKABLE void removeFromVehicle();    
+    Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString)
+    Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)
+
 
     Q_INVOKABLE void saveToCurrent  ();
     Q_INVOKABLE void saveToFile     (const QString& filename);
@@ -148,6 +153,9 @@ signals:
     void uploadOverrideRequiredChanged  (void);
     void phaseDistanceChanged           (void);
     void phaseDurationChanged           (void);
+    void uploadAndExecuteConfirmRequired(void);
+    void vehicleHasLowBatteryChanged    (void);
+    void returnBatteryLowConfirmRequired(void);
 
 private slots:
     bool fetchContainerData();
@@ -159,10 +167,17 @@ private slots:
     bool setTakeoffLandPosition             (void);
     void updateSpeed                        (void);
     void updateAltitude                     (void);
+    void checkBatteryLevel                  (void);
 
 private:
     void _setPhaseDistance(double distance);
     void _setPhaseDuration(double duration);
+    bool _checkSmartRTLPreCondition(QString &errorString); // should be called from gui, befor calcReturnPath()
+    bool _calcReturnPath(QString &errorSring); // Calculates return path (destination: service area center) for a flying vehicle
+    void _setVehicleHasLowBattery(bool batteryLow);
+
+
+
 
 
 private:
@@ -179,6 +194,7 @@ private:
     QmlObjectListModel      _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView
     QmlObjectListModel      _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
                                                   // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage
+    QmlObjectListModel      _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked
     QVariantList            _waypointPath; // path connecting the items in _missionItems
     QVariantList            _currentWaypointPath; // path connecting the items in _currentMissionItems
     QGeoCoordinate          _takeoffLandPostion;
@@ -204,6 +220,10 @@ private:
                                                           // The user can override the upload lock with a slider, this will reset this variable to false.
     double                       _phaseDistance; // the lenth in meters of the current phase
     double                       _phaseDuration; // the phase duration in seconds
+
+    QTimer                       _checkBatteryTimer;
+    bool                         _vehicleHasLowBattery;
+    bool                         _lowBatteryHandlingTriggered;
 };
 
 

src/Wima/WimaPlanData.cc

@@ -100,9 +100,12 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData)
     }
 }
 
-void WimaPlanData::append(const QList<const MissionItem *> &missionItems)
+void WimaPlanData::append(const QList<MissionItem *> &missionItems)
 {
-    _missionItems.append(missionItems);
+    for (MissionItem *item : missionItems) {
+        MissionItem copy = MissionItem(*item, this);
+        _missionItems.append(copy);
+    }
 }
 
 /*!
@@ -121,7 +124,7 @@ QList<const WimaAreaData *> WimaPlanData::areaList() const
     return _areaList;
 }
 
-QList<const MissionItem *> WimaPlanData::missionItems() const
+QList<MissionItem> WimaPlanData::missionItems() const
 {
     return _missionItems;
 }

src/Wima/WimaPlanData.h

@@ -22,17 +22,20 @@ public:
     void append(const WimaServiceAreaData       &areaData);
     void append(const WimaCorridorData          &areaData);
     void append(const WimaMeasurementAreaData   &areaData);
-    void append(const QList<const MissionItem*> &missionItems);
+    void append(const QList<MissionItem *> &missionItems);
     void clear();
 
     QList<const WimaAreaData *> areaList() const;
-    QList<const MissionItem *>  missionItems() const;
+    QList<MissionItem>  missionItems() const;
 
 
 
 signals:
     void areaListChanged();
 
+private:
+    void _clearAndDeleteMissionItems();
+
 private:
     WimaJoinedAreaData      _joinedArea;
     WimaServiceAreaData     _serviceArea;
@@ -40,5 +43,5 @@ private:
     WimaMeasurementAreaData _measurementArea;
     QList<const WimaAreaData*>    _areaList;
 
-    QList<const MissionItem*>           _missionItems;
+    QList<MissionItem>           _missionItems;
 };

src/Wima/WimaPlaner.cc

@@ -701,17 +701,12 @@ WimaPlanData WimaPlaner::toPlanData()
     planData.append(WimaJoinedAreaData(_joinedArea));
 
     // convert mission items to mavlink commands
+    QObject deleteObject; // used to automatically delete content of rgMissionItems after this function call
     QList<MissionItem*> rgMissionItems;
-    MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this);
-
-    // add const qualifier...
-    QList<const MissionItem*> rgMissionItemsConst;
-    for (int i = _arrivalPathLength + 1; i < rgMissionItems.size() - _returnPathLength; i++) { // i = _arrivalPathLength + 1: + 1 = MissionSettingsItem ...
-        rgMissionItemsConst.append(rgMissionItems.value(i));
-    }
+    MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, &deleteObject);
 
     // store mavlink commands
-    planData.append(rgMissionItemsConst);
+    planData.append(rgMissionItems);
 
     return planData;
 }