diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 2c41ff26bdebdd635b14b1c0bea07b1d5edaf0c5..65c9055e6e0788ad4079e79f169928addf93c08c 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -121,7 +121,7 @@ QGCView { wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer }) } - onUploadAndExecuteConfirmRequired: { + onReturnUserRequestConfirmRequired: { _guidedController.confirmAction(_guidedController.actionUploadAndExecute) } @@ -615,12 +615,6 @@ QGCView { wimaController: _wimaController planMasterController: _planMasterController missionController: _missionController - - onInitSmartRTL: { - if (wimaController.checkSmartRTLPreCondition()) { - _guidedController.confirmAction(_guidedController.actionInitSmartRTL) - } - } } //------------------------------------------------------------------------- diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 02f55e884044acc2bb1ea5d4413e587676d2e991..24ba8dad8409f20cfd456c4755b9b57cb3fba307 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -35,7 +35,6 @@ Item { property real _controllerProgressPct: _controllerValid ? planMasterController.missionController.progressPct : 0 property real _margins: ScreenTools.defaultFontPixelWidth / 2 - signal initSmartRTL(); DeadMouseArea { anchors.fill: parent @@ -316,7 +315,7 @@ Item { QGCButton { id: buttonSmartRTL text: qsTr("Smart RTL") - onClicked: initSmartRTL() + onClicked: wimaController.initSmartRTL(); Layout.columnSpan: 2 Layout.fillWidth: true } diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 054b554e1156df05371c7a28fa5812761541222f..d69bce12d3743ebbdcaadba5bb5775b4228833c2 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -54,7 +54,6 @@ 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") @@ -77,7 +76,6 @@ Item { 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 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?") @@ -103,8 +101,7 @@ Item { readonly property int actionMVStartMission: 19 readonly property int actionVtolTransitionToFwdFlight: 20 readonly property int actionVtolTransitionToMRFlight: 21 - readonly property int actionOverrideUpload: 22 - readonly property int actionInitSmartRTL: 23 + readonly property int actionOverrideUpload: 22 readonly property int actionUploadAndExecute: 24 readonly property int actionReturnBatteryLow: 25 // very similar to actionUploadAndExecute, is triggered on low battery @@ -132,7 +129,7 @@ Item { property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && _activeVehicle) ? _rcRSSIAvailable : _activeVehicle property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property string _flightMode: _activeVehicle ? _activeVehicle.flightMode : "" - property bool _missionAvailable: wimaEnabled ? wimaMenu.missionReadyForStart : missionController.containsItems + property bool _missionAvailable: missionController.containsItems && missionController.progressPct === 1 property bool _missionActive: _activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false @@ -352,11 +349,6 @@ 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 @@ -445,9 +437,6 @@ Item { case actionOverrideUpload: wimaController.forceUploadToVehicle() break - case actionInitSmartRTL: - wimaController.initSmartRTL() - break case actionUploadAndExecute: case actionReturnBatteryLow: wimaController.executeSmartRTL() diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 31a3dd9b6e8cbb3d67535df132ac9afbdaff9616..e0d16655f76bac8304356a27f07f7f38c77fc8a0 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -88,7 +88,6 @@ MissionController::MissionController(PlanMasterController* masterController, QOb _remainingDistanceTimeTimer.setInterval(REMAINING_DIST_TIME_UPDATE_INTERVAL); connect(&_remainingDistanceTimeTimer, &QTimer::timeout, this, &MissionController::_updateRemainingDistanceTime); - } MissionController::~MissionController() @@ -243,6 +242,24 @@ void MissionController::_warnIfTerrainFrameUsed(void) } } +void MissionController::_setRemainingDistance(double dist) +{ + if (qFuzzyCompare(dist, _remainingDistance) == false) { + _remainingDistance = dist; + + emit remainingDistanceChanged(); + } +} + +void MissionController::_setRemainingTime(double time) +{ + if (qFuzzyCompare(time, _remainingTime) == false) { + _remainingTime = time; + + emit remainingTimeChanged(); + } +} + void MissionController::sendToVehicle(void) { if (_masterController->offline()) { @@ -1680,11 +1697,8 @@ void MissionController::_enableDisableRemainingDistTimeCalculation(bool flying) _remainingDistanceTimeTimer.start(); } else { _remainingDistanceTimeTimer.stop(); - _remainingTime = -1; - _remainingDistance = -1; - - emit remainingTimeChanged(); - emit remainingDistanceChanged(); + _setRemainingTime(-1); + _setRemainingDistance(-1); } } @@ -1694,11 +1708,11 @@ void MissionController::_updateRemainingDistanceTime() double time = 0; bool success = distanceTimeToMissionEnd(dist, time, _missionManager->currentIndex() /*missionIndex >= 1*/, true /* useVehiclePostion */); if (success) { - _remainingTime = time; - _remainingDistance = dist; - - emit remainingTimeChanged(); - emit remainingDistanceChanged(); + _setRemainingTime(time); + _setRemainingDistance(dist); + } else { + _setRemainingTime(-1); + _setRemainingDistance(-1); } } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 992a4e53e94c793cfbc13afd7a005d01955b4202..9ce1bb923e2b712f3b8c113e53a6baa326d1014d 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -318,6 +318,8 @@ private: void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum); int _insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i); void _warnIfTerrainFrameUsed(void); + void _setRemainingDistance(double dist); + void _setRemainingTime(double time); private: MissionManager* _missionManager; diff --git a/src/Settings/Wima.SettingsGroup.json b/src/Settings/Wima.SettingsGroup.json index 54aacd736e7299bf28dd26fbf3188477dc28c156..9f9275c4f08b18a24173a7b5596c2126a0707e87 100644 --- a/src/Settings/Wima.SettingsGroup.json +++ b/src/Settings/Wima.SettingsGroup.json @@ -4,7 +4,7 @@ "shortDescription": "The battery threshold in percents, for which low battery handling measures get triggered.", "type": "double", "units": "%", - "defaultValue": 40 + "defaultValue": 35 }, { "name": "enableLowBatteryHandling", @@ -17,6 +17,6 @@ "shortDescription": "If the remaining estimated mission time is lower than this value, low battery handling will not be triggered.", "type": "uint64", "units": "s", - "defaultValue": 60 + "defaultValue": 15 } ] diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 53db294864abef9a46450a6970b5935c23c872e2..7fa27faa0aad86a069d38a861a5ef673ff1756ed 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -37,16 +37,16 @@ WimaController::WimaController(QObject *parent) , _endWaypointIndex (0) , _startWaypointIndex (0) , _uploadOverrideRequired (false) - , _measurementPathLength (-1) + , _measurementPathLength (-1) , _arrivalPathLength (-1) , _returnPathLength (-1) , _phaseDistance (-1) , _phaseDuration (-1) , _phaseDistanceBuffer (-1) , _phaseDurationBuffer (-1) - , _vehicleHasLowBattery (false) - , _lowBatteryHandlingTriggered(false) - , _executingSmartRTL (false) + , _vehicleHasLowBattery (false) + , _lowBatteryHandlingTriggered (false) + , _executingSmartRTL (false) { _showAllMissionItems.setRawValue(true); @@ -61,7 +61,9 @@ WimaController::WimaController(QObject *parent) // setup low battery handling connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel); - _checkBatteryTimer.setInterval(500); + _checkBatteryTimer.setInterval(CHECK_BATTERY_INTERVAL); + + Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::enableDisableLowBatteryHandling); enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue()); @@ -343,26 +345,15 @@ bool WimaController::calcReturnPath() return true; } -bool WimaController::executeSmartRTL() +void WimaController::executeSmartRTL() { - QString errorString; - bool retValue = _executeSmartRTL(errorString); - if (!retValue) { - qgcApp()->showMessage(errorString); - } - - return retValue; + _executeSmartRTL(); } -bool WimaController::initSmartRTL() +void WimaController::initSmartRTL() { - masterController()->managerVehicle()->pauseVehicle(); - QString errorString; - bool retValue = calcReturnPath(); - if (!retValue) - return false; - emit uploadAndExecuteConfirmRequired(); - return true; + _srtlReason = UserRequest; + _initSmartRTL(); } void WimaController::removeVehicleTrajectoryHistory() @@ -928,8 +919,6 @@ void WimaController::checkBatteryLevel() bool enabled = _enableWimaController.rawValue().toBool(); unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt(); - static short attemptCounter = 0; - if (managerVehicle != nullptr && enabled == true) { Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); @@ -944,26 +933,9 @@ void WimaController::checkBatteryLevel() _lowBatteryHandlingTriggered = true; } else { - QString errorString; - attemptCounter++; - if (_checkSmartRTLPreCondition(errorString) == true) { - - managerVehicle->pauseVehicle(); - if (_calcReturnPath(errorString)) { - _lowBatteryHandlingTriggered = true; - attemptCounter = 0; - emit returnBatteryLowConfirmRequired(); - } else { - if (attemptCounter > 3) { - qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible.")); - qgcApp()->showMessage(errorString); - } - } - } - if (attemptCounter > 3) { // try 3 times, somtimes vehicle is outside joined area - _lowBatteryHandlingTriggered = true; - attemptCounter = 0; - } + _lowBatteryHandlingTriggered = true; + _srtlReason = BatteryLow; + _initSmartRTL(); } } } @@ -1035,10 +1007,15 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString) return false; } Vehicle *managerVehicle = masterController()->managerVehicle(); - if (!managerVehicle->flying()) { - errorString.append(tr("Vehicle is not flying. Smart RTL not available.")); - return false; - } + if (!managerVehicle->flying()) { + errorString.append(tr("Vehicle is not flying. Smart RTL not available.")); + return false; + } + + if (!_joinedArea.containsCoordinate(managerVehicle->coordinate())) { + errorString.append(tr("Vehicle not inside save area. Smart RTL not available.")); + return false; + } return true; } @@ -1136,8 +1113,12 @@ bool WimaController::_calcReturnPath(QString &errorSring) } //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); - _setPhaseDistance(-1); - _setPhaseDuration(-1); + double dist = 0; + double time = 0; + if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false)) + qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!"); + _setPhaseDistance(dist); + _setPhaseDuration(time); _missionController->removeAll(); // remove items from _missionController, will be added on upload updateAltitude(); @@ -1161,15 +1142,46 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow) } } -bool WimaController::_executeSmartRTL(QString &errorSring) +void WimaController::_initSmartRTL() +{ + QString errorString; + static int attemptCounter = 0; + attemptCounter++; + + if (_checkSmartRTLPreCondition(errorString) == true) { + _masterController->managerVehicle()->pauseVehicle(); + connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp); + if (_calcReturnPath(errorString)) { + _executingSmartRTL = true; + attemptCounter = 0; + + switch(_srtlReason) { + case BatteryLow: + emit returnBatteryLowConfirmRequired(); + break; + case UserRequest: + emit returnUserRequestConfirmRequired(); + break; + default: + qWarning("\nWimaController::_initSmartRTL: default case reached!"); + } + + return; + } + } + if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area + errorString.append(tr("Smart RTL: No success after maximum number of attempts.")); + qgcApp()->showMessage(errorString); + attemptCounter = 0; + } else { + _smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL); + } +} + +void WimaController::_executeSmartRTL() { - Q_UNUSED(errorSring) - _executingSmartRTL = true; - connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp); forceUploadToVehicle(); masterController()->managerVehicle()->startMission(); - - return true; } void WimaController::_loadCurrentMissionItemsFromBuffer() diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 4a4a085fb4bf771f001d74c9913d104392c2fe60..72021a2e314a763232cf21a3c35251d5e759af1b 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -24,6 +24,10 @@ #include "WimaSettings.h" #include "SettingsManager.h" +#define CHECK_BATTERY_INTERVAL 1000 +#define SMART_RTL_MAX_ATTEMPTS 3 +#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms + class WimaController : public QObject { @@ -109,8 +113,8 @@ public: Q_INVOKABLE void removeFromVehicle(); Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString) Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)# - Q_INVOKABLE bool executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring) - Q_INVOKABLE bool initSmartRTL(); + Q_INVOKABLE void executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring) + Q_INVOKABLE void initSmartRTL(); Q_INVOKABLE void removeVehicleTrajectoryHistory(); @@ -151,23 +155,24 @@ public: bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex); signals: - void masterControllerChanged (void); - void missionControllerChanged (void); - void visualItemsChanged (void); - void currentFileChanged (); - void dataContainerChanged (); - void readyForSaveSendChanged (bool ready); - void missionItemsChanged (void); - void currentMissionItemsChanged (void); - void waypointPathChanged (void); - void currentWaypointPathChanged (void); - void uploadOverrideRequiredChanged (void); - void phaseDistanceChanged (void); - void phaseDurationChanged (void); - void uploadAndExecuteConfirmRequired(void); - void vehicleHasLowBatteryChanged (void); - void returnBatteryLowConfirmRequired(void); - + void masterControllerChanged (void); + void missionControllerChanged (void); + void visualItemsChanged (void); + void currentFileChanged (); + void dataContainerChanged (); + void readyForSaveSendChanged (bool ready); + void missionItemsChanged (void); + void currentMissionItemsChanged (void); + void waypointPathChanged (void); + void currentWaypointPathChanged (void); + void uploadOverrideRequiredChanged (void); + void phaseDistanceChanged (void); + void phaseDurationChanged (void); + void vehicleHasLowBatteryChanged (void); + void returnBatteryLowConfirmRequired (void); + void returnUserRequestConfirmRequired (void); +private: + enum SRTL_Reason {BatteryLow, UserRequest}; private slots: bool fetchContainerData(); bool calcNextPhase(void); @@ -183,6 +188,9 @@ private slots: void smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL void enableDisableLowBatteryHandling (QVariant enable); void reverseChangedHandler (); + void _initSmartRTL (); + void _executeSmartRTL (); + private: void _setPhaseDistance(double distance); @@ -190,7 +198,6 @@ private: 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); - bool _executeSmartRTL(QString &errorSring); void _loadCurrentMissionItemsFromBuffer(); void _saveCurrentMissionItemsToBuffer(); @@ -242,6 +249,9 @@ private: double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL QTimer _checkBatteryTimer; + QTimer _smartRTLAttemptTimer; + SRTL_Reason _srtlReason; + bool _vehicleHasLowBattery; bool _lowBatteryHandlingTriggered; bool _executingSmartRTL;