diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 2735e698476e45227c50343bef676966f11384de..0c5f001a1718abde4cd2be8d1c12bca2b73f4dd2 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/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) + } + } } //------------------------------------------------------------------------- diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 23243c39293dd3c2ea9b31f407380805659d9357..57811710d4f1c546f27038f5968c3201a81d76a1 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/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 diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 2261691bc9da77b3cca3ac409903c4626b50a39e..59d443e44011dd9cc133fb3aaf80124d0d802b0b 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/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 diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 2a99bf256121925933765956af481e32521dec63..ef725e6e91a7d04f7dc6d4b044c195fd8666cee4 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -35,6 +35,8 @@ WimaController::WimaController(QObject *parent) , _uploadOverrideRequired (false) , _phaseDistance(-1) , _phaseDuration(-1) + , _vehicleHasLowBattery(false) + , _lowBatteryHandlingTriggered(false) { _nextPhaseStartWaypointIndex.setRawValue(int(1)); @@ -45,6 +47,12 @@ WimaController::WimaController(QObject *parent) connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateSpeed); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude); + + // battery timer + connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel); + + _checkBatteryTimer.setInterval(500); + _checkBatteryTimer.start(); } QmlObjectListModel* WimaController::visualItems() @@ -144,6 +152,11 @@ double WimaController::phaseDuration() const return _phaseDuration; } +bool WimaController::vehicleHasLowBattery() const +{ + return _vehicleHasLowBattery; +} + Fact *WimaController::startWaypointIndex() { return &_nextPhaseStartWaypointIndex; @@ -252,6 +265,29 @@ bool WimaController::forceUploadToVehicle() void WimaController::removeFromVehicle() { _masterController->removeAllFromVehicle(); + _missionController->removeAll(); +} + +bool WimaController::checkSmartRTLPreCondition() +{ + QString errorString; + bool retValue = _checkSmartRTLPreCondition(errorString); + if (retValue == false) { + qgcApp()->showMessage(errorString); + return false; + } + return true; +} + +bool WimaController::calcReturnPath() +{ + QString errorString; + bool retValue = _calcReturnPath(errorString); + if (retValue == false) { + qgcApp()->showMessage(errorString); + return false; + } + return true; } void WimaController::saveToCurrent() @@ -345,15 +381,15 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex); + if (!retValue) + return false; + for (int i = 0; i < geoCoordintateList.size(); i++) { QGeoCoordinate vertex = geoCoordintateList[i]; if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude())) geoCoordintateList.removeAt(i); } - if (!retValue) - return false; - for (auto coordinate : geoCoordintateList) coordinateList.append(QVariant::fromValue(coordinate)); @@ -373,8 +409,8 @@ bool WimaController::fetchContainerData() // reset visual items _visualItems.clear(); - _missionItems.clear(); - _currentMissionItems.clear(); + _missionItems.clearAndDeleteContents(); + _currentMissionItems.clearAndDeleteContents(); _waypointPath.clear(); _currentWaypointPath.clear(); @@ -438,7 +474,7 @@ bool WimaController::fetchContainerData() } // extract mission items - QList tempMissionItems = planData.missionItems(); + QList tempMissionItems = planData.missionItems(); if (tempMissionItems.size() < 1) return false; @@ -448,8 +484,7 @@ bool WimaController::fetchContainerData() // create SimpleMissionItem by using _missionController for ( int i = 0; i < tempMissionItems.size(); i++) { - const MissionItem *missionItem = tempMissionItems[i]; - _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); + _missionController->insertSimpleMissionItem(tempMissionItems[i], missionControllerVisualItems->count()); } // copy mission items from _missionController to _missionItems for ( int i = 1; i < missionControllerVisualItems->count(); i++) { @@ -492,7 +527,7 @@ bool WimaController::calcNextPhase() if (_missionItems.count() < 1 || _lastMissionPhaseReached) return false; - _currentMissionItems.clear(); + _currentMissionItems.clearAndDeleteContents(); _currentWaypointPath.clear(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); @@ -683,6 +718,40 @@ void WimaController::updateAltitude() } } +void WimaController::checkBatteryLevel() +{ + Vehicle *managerVehicle = masterController()->managerVehicle(); + int batteryThreshold = 94; // percent + if (managerVehicle != nullptr) { + Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); + Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); + + + if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold + && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) { + _setVehicleHasLowBattery(true); + if (!_lowBatteryHandlingTriggered) { + QString errorString; + if (_checkSmartRTLPreCondition(errorString) == true) { + managerVehicle->pauseVehicle(); + if (_calcReturnPath(errorString)) { + emit returnBatteryLowConfirmRequired(); + } else { + qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible.")); + qgcApp()->showMessage(errorString); + } + } + _lowBatteryHandlingTriggered = true; + } + } + else { + _setVehicleHasLowBattery(false); + _lowBatteryHandlingTriggered = false; + } + + } +} + void WimaController::_setPhaseDistance(double distance) { if (!qFuzzyCompare(distance, _phaseDistance)) { @@ -701,5 +770,143 @@ void WimaController::_setPhaseDuration(double duration) } } +bool WimaController::_checkSmartRTLPreCondition(QString &errorString) +{ + if (!_localPlanDataValid) { + errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area.")); + return false; + } + Vehicle *managerVehicle = masterController()->managerVehicle(); + if (!managerVehicle->flying()) { + errorString.append(tr("Vehicle is not flying. Smart RTL not available.")); + return false; + } +} + +bool WimaController::_calcReturnPath(QString &errorSring) +{ + // it is assumed that checkSmartRTLPreCondition() was called first and true was returned + + + Vehicle *managerVehicle = masterController()->managerVehicle(); + + QGeoCoordinate currentVehiclePosition = managerVehicle->coordinate(); + // check if vehicle inside _joinedArea, this statement is not inside checkSmartRTLPreCondition() because during checkSmartRTLPreCondition() vehicle is not paused yet + if (!_joinedArea.containsCoordinate(currentVehiclePosition)) { + errorSring.append(tr("Vehicle not inside joined area. Action not supported.")); + return false; + } + + // calculate return path + QList returnPath; + calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath); + // successful? + if (returnPath.isEmpty()) { + errorSring.append(tr("Not able to calculate return path.")); + return false; + } + // qWarning() << "returnPath.size()" << returnPath.size(); + + // buffer _currentMissionItems + //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); + //qWarning() << "_missionItemsBuffer.count()" << _missionItemsBuffer.count(); + _missionItemsBuffer.clear(); + int numCurrentMissionItems = _currentMissionItems.count(); + for (int i = 0; i < numCurrentMissionItems; i++) + _missionItemsBuffer.append(_currentMissionItems.removeAt(0)); + //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); + //qWarning() << "_missionItemsBuffer.count()" << _missionItemsBuffer.count(); + + // create Mission Items + removeFromVehicle(); + QmlObjectListModel* missionControllerVisuals = _missionController->visualItems(); + + // set homeposition of settingsItem + MissionSettingsItem* settingsItem = missionControllerVisuals->value(0); + if (settingsItem == nullptr) { + qWarning("WimaController: nullptr"); + return false; + } + settingsItem->setCoordinate(_takeoffLandPostion); + + // copy from returnPath to _missionController + QGeoCoordinate speedItemCoordinate = returnPath.first(); + for (auto coordinate : returnPath) { + _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); + } + //qWarning() << "missionControllerVisuals->count()" << missionControllerVisuals->count(); + + // create speed item + int speedItemIndex = 1; + _missionController->insertSimpleMissionItem(speedItemCoordinate, speedItemIndex); + SimpleMissionItem *speedItem = missionControllerVisuals->value(speedItemIndex); + if (speedItem == nullptr) { + qWarning("WimaController: nullptr"); + return false; + } + speedItem->setCoordinate(speedItemCoordinate); + speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); + speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); + + // set second item command to ordinary waypoint (is takeoff) + SimpleMissionItem *secondItem = missionControllerVisuals->value(2); + if (secondItem == nullptr) { + qWarning("WimaController: nullptr"); + return false; + } + secondItem->setCoordinate(speedItemCoordinate); + secondItem->setCommand(MAV_CMD_NAV_WAYPOINT); + + + // set land command for last mission item + SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); + if (landItem == nullptr) { + qWarning("WimaController: nullptr"); + return false; + } + _missionController->setLandCommand(*landItem); + + // copy to _currentMissionItems + //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); + for ( int i = 1; i < missionControllerVisuals->count(); i++) { + SimpleMissionItem *visualItem = missionControllerVisuals->value(i); + if (visualItem == nullptr) { + qWarning("WimaController: Nullptr at SimpleMissionItem!"); + _currentMissionItems.clear(); + return false; + } + + SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); + _currentMissionItems.append(visualItemCopy); + } + //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); + + _setPhaseDistance(_missionController->missionDistance()); + _setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble()); + _missionController->removeAll(); // remove items from _missionController, will be added on upload + updateAltitude(); + + updateCurrentPath(); + emit currentMissionItemsChanged(); + + + //qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count(); + _showAllMissionItems.setRawValue(false); + managerVehicle->trajectoryPoints()->clear(); + + emit uploadAndExecuteConfirmRequired(); + return true; +} + +void WimaController::_setVehicleHasLowBattery(bool batteryLow) +{ + if (_vehicleHasLowBattery != batteryLow) { + _vehicleHasLowBattery = batteryLow; + + emit vehicleHasLowBatteryChanged(); + qWarning() << "WimaController::_setVehicleHasLowBattery(bool batteryLow)" << _vehicleHasLowBattery; + } +} + diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 879a70a8e2676685636c057a479d9607b9a5a937..4dcbe2c363c969f48a4e79c8ada6954bff70d9bb 100644 --- a/src/Wima/WimaController.h +++ b/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; }; diff --git a/src/Wima/WimaPlanData.cc b/src/Wima/WimaPlanData.cc index b379f4eb573d48bf9c166618f9f2712f71a81518..ab15fa63870225a046c3a9ce7534985ad79bdc43 100644 --- a/src/Wima/WimaPlanData.cc +++ b/src/Wima/WimaPlanData.cc @@ -100,9 +100,12 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData) } } -void WimaPlanData::append(const QList &missionItems) +void WimaPlanData::append(const QList &missionItems) { - _missionItems.append(missionItems); + for (MissionItem *item : missionItems) { + MissionItem copy = MissionItem(*item, this); + _missionItems.append(copy); + } } /*! @@ -121,7 +124,7 @@ QList WimaPlanData::areaList() const return _areaList; } -QList WimaPlanData::missionItems() const +QList WimaPlanData::missionItems() const { return _missionItems; } diff --git a/src/Wima/WimaPlanData.h b/src/Wima/WimaPlanData.h index c283797e695153f7d7576965b23a7882f580deba..eb20089c23b7d7d06c33f8e0c8992b86c9e90ec6 100644 --- a/src/Wima/WimaPlanData.h +++ b/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 &missionItems); + void append(const QList &missionItems); void clear(); QList areaList() const; - QList missionItems() const; + QList missionItems() const; signals: void areaListChanged(); +private: + void _clearAndDeleteMissionItems(); + private: WimaJoinedAreaData _joinedArea; WimaServiceAreaData _serviceArea; @@ -40,5 +43,5 @@ private: WimaMeasurementAreaData _measurementArea; QList _areaList; - QList _missionItems; + QList _missionItems; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 4d2d67d057aa719ff597010d9c110ce4f77f83d9..e8f979fa7cf9f96dbca4c486b69b102cc7106ef5 100644 --- a/src/Wima/WimaPlaner.cc +++ b/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 rgMissionItems; - MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this); - - // add const qualifier... - QList 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; }