diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 96ad0ab6414ff805405ba638800d5d304dc2a7d2..f4dd8dcf6b1981d7888dcf69496309f475ee16fc 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -58,6 +58,10 @@ FlightMap { property bool _disableVehicleTracking: false property bool _keepVehicleCentered: _mainIsMap ? false : true + property bool _wimaEnabled: wimaController.enableWimaController.value + property bool _showAllWimaItems: wimaController.showAllMissionItems.value + property bool _showCurrentWimaItems: wimaController.showCurrentMissionItems.value + function updateAirspace(reset) { if(_airspaceEnabled) { var coordinateNW = flightMap.toCoordinate(Qt.point(0,0), false /* clipToViewPort */) @@ -193,13 +197,6 @@ FlightMap { property real leftToolWidth: toolStrip.x + toolStrip.width } - // Add mission items generated by wima planer to the map - WimaPlanMapItems { - map: flightMap - largeMapView: _mainIsMap - wimaController: flightMap.wimaController - } - // Add wima Areas to the Map MapItemView { property bool _enableWima: wimaController.enableWimaController.value @@ -214,10 +211,33 @@ FlightMap { : object.type === "WimaMeasurementAreaData" ? "green" : "transparent" opacity: 0.25 - z: QGroundControl.zOrderWaypointLines-2 + z: 0 } } + // Add mission items generated by wima planer to the map + WimaPlanMapItems { + map: flightMap + largeMapView: _mainIsMap + missionItems: wimaController.missionItems + path: wimaController.waypointPath + showItems: _wimaEnabled && _showAllWimaItems + zOrderWP: QGroundControl.zOrderWaypointIndicators-2 + zOrderLines: QGroundControl.zOrderWaypointLines-2 + color: "#B4808080" // gray with alpha 0.7 + } + + WimaPlanMapItems { + map: flightMap + largeMapView: _mainIsMap + missionItems: wimaController.currentMissionItems + path: wimaController.currentWaypointPath + showItems: _wimaEnabled && _showCurrentWimaItems + zOrderWP: QGroundControl.zOrderWaypointIndicators-1 + zOrderLines: QGroundControl.zOrderWaypointLines-1 + color: "green" // gray with alpha 0.7 + } + // Add trajectory points to the map MapItemView { model: _mainIsMap ? _activeVehicle ? _activeVehicle.trajectoryPoints : 0 : 0 diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 59d443e44011dd9cc133fb3aaf80124d0d802b0b..a34ca998046346906bd08e83b7f5942970b76f2a 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -444,16 +444,11 @@ Item { wimaController.forceUploadToVehicle() break case actionInitSmartRTL: - _activeVehicle.pauseVehicle() - wimaController.calcReturnPath() + wimaController.initSmartRTL() break case actionUploadAndExecute: - wimaController.forceUploadToVehicle() - _activeVehicle.startMission() - break case actionReturnBatteryLow: - wimaController.forceUploadToVehicle() - _activeVehicle.startMission() + wimaController.executeSmartRTL() break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) diff --git a/src/FlightMap/MapItems/WimaPlanMapItems.qml b/src/FlightMap/MapItems/WimaPlanMapItems.qml index 205a27a158468bf6212f6468c3b09143a8e8165f..7237f32b038e185ded84b0b057c902455a7f08ce 100644 --- a/src/FlightMap/MapItems/WimaPlanMapItems.qml +++ b/src/FlightMap/MapItems/WimaPlanMapItems.qml @@ -21,85 +21,51 @@ Item { property var map ///< Map control to show items on property bool largeMapView ///< true: map takes up entire view, false: map is in small window - property var wimaController - property string mIColor: "#B4808080" // Mission Items Color, gray with alpha 0.7 #AARRGGBB - property string mIlineColor: mIColor - property string cMIColor: "orangered" // Current Mission Items Color - property string cMIlineColor: cMIColor + property string color: "green" // Mission Items Color + property var missionItems + property var path + property var zOrderWP + property var zOrderLines property var _map: map property var _missionLineViewComponent - property var _currentMissionLineViewComponent - property bool _showAllItems: wimaController.showAllMissionItems.value - property bool _showCurrentItems: wimaController.showCurrentMissionItems.value - property bool _wimaEnabled: wimaController.enableWimaController.value + property bool showItems: true - // Add the mission item visuals to the map - Repeater { - model: largeMapView ? (_showAllItems ? (_wimaEnabled ? wimaController.missionItems : 0) : 0) : 0 - z: QGroundControl.zOrderWaypointIndicators-2 + Component { + id: missionLineViewComponent - delegate: WimaMissionItemMapVisual { - map: _map - color: _root.mIColor -// onClicked: { -// if (isActiveVehicle) { -// // Only active vehicle supports click to change current mission item -// guidedActionsController.confirmAction(guidedActionsController.actionSetWaypoint, Math.max(object.sequenceNumber, 1)) -// } -// } + MapPolyline { + line.width: 3 + line.color: showItems ? _root.color : "transparent" + path: _root.path + z: _root.zOrderLines } } - // Add the current mission item visuals to the map + Repeater { - model: largeMapView ? (_showCurrentItems ? (_wimaEnabled ? wimaController.currentMissionItems :0 ) : 0) : 0 - z: QGroundControl.zOrderWaypointIndicators-1 + model: largeMapView ? ( showItems ? missionItems : 0) : 0 delegate: WimaMissionItemMapVisual { map: _map - color: _root.cMIColor + color: _root.color + zOrder: _root.zOrderWP } } + Component.onCompleted: { _missionLineViewComponent = missionLineViewComponent.createObject(map) if (_missionLineViewComponent.status === Component.Error) console.log(_missionLineViewComponent.errorString()) map.addMapItem(_missionLineViewComponent) - - _currentMissionLineViewComponent = currentMissionLineViewComponent.createObject(map) - if (_currentMissionLineViewComponent.status === Component.Error) - console.log(_currentMissionLineViewComponent.errorString()) - map.addMapItem(_currentMissionLineViewComponent) } Component.onDestruction: { _missionLineViewComponent.destroy() - _currentMissionLineViewComponent.destroy() - } - - Component { - id: missionLineViewComponent - - MapPolyline { - line.width: 3 - line.color: _showAllItems ? (_wimaEnabled ? mIlineColor : "transparent"): "transparent" - z: QGroundControl.zOrderWaypointLines-2 - path: wimaController.waypointPath - } } - Component { - id: currentMissionLineViewComponent - MapPolyline { - line.width: 3 - line.color: _showCurrentItems ? (_wimaEnabled ? cMIlineColor : "transparent") : "transparent" - z: QGroundControl.zOrderWaypointLines-1 - path: wimaController.currentWaypointPath - } - } } diff --git a/src/PlanView/WimaMissionItemMapVisual.qml b/src/PlanView/WimaMissionItemMapVisual.qml index bd26bb902ad34518268b0af8e5c1c8fff6f0dfcc..b70a0d6fc2cee2f458849412a3f9ec5ac13db80b 100644 --- a/src/PlanView/WimaMissionItemMapVisual.qml +++ b/src/PlanView/WimaMissionItemMapVisual.qml @@ -24,6 +24,7 @@ Item { property var map ///< Map control to place item in property var qgcView ///< QGCView to use for popping dialogs + property int zOrder property var color property var _missionItem: object @@ -107,7 +108,7 @@ Item { MissionItemIndicator { coordinate: _missionItem.coordinate visible: _missionItem.specifiesCoordinate - z: QGroundControl.zOrderMapItems + z: _root.zOrder missionItem: _missionItem sequenceNumber: _missionItem.sequenceNumber color: _root.color diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index ef725e6e91a7d04f7dc6d4b044c195fd8666cee4..a82b88eed8f4dea16d64893fa25c3fecb0a6570d 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -37,6 +37,7 @@ WimaController::WimaController(QObject *parent) , _phaseDuration(-1) , _vehicleHasLowBattery(false) , _lowBatteryHandlingTriggered(false) + , _executingSmartRTL(false) { _nextPhaseStartWaypointIndex.setRawValue(int(1)); @@ -50,7 +51,6 @@ WimaController::WimaController(QObject *parent) // battery timer connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel); - _checkBatteryTimer.setInterval(500); _checkBatteryTimer.start(); } @@ -290,6 +290,27 @@ bool WimaController::calcReturnPath() return true; } +bool WimaController::executeSmartRTL() +{ + QString errorString; + bool retValue = _executeSmartRTL(errorString); + if (!retValue) { + qgcApp()->showMessage(errorString); + } + + return retValue; +} + +bool WimaController::initSmartRTL() +{ + masterController()->managerVehicle()->pauseVehicle(); + QString errorString; + bool retValue = calcReturnPath(); + if (!retValue) + return false; + return true; +} + void WimaController::saveToCurrent() { @@ -734,14 +755,14 @@ void WimaController::checkBatteryLevel() QString errorString; if (_checkSmartRTLPreCondition(errorString) == true) { managerVehicle->pauseVehicle(); - if (_calcReturnPath(errorString)) { + if (_calcReturnPath(errorString)) { + _lowBatteryHandlingTriggered = true; emit returnBatteryLowConfirmRequired(); } else { qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible.")); qgcApp()->showMessage(errorString); } } - _lowBatteryHandlingTriggered = true; } } else { @@ -752,6 +773,19 @@ void WimaController::checkBatteryLevel() } } +void WimaController::smartRTLCleanUp(bool flying) +{ + + if ( !flying) { // vehicle has landed + if (_executingSmartRTL) { + _executingSmartRTL = false; + _loadCurrentMissionItemsFromBuffer(); + _showAllMissionItems.setRawValue(true); + disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp); + } + } +} + void WimaController::_setPhaseDistance(double distance) { if (!qFuzzyCompare(distance, _phaseDistance)) { @@ -781,6 +815,8 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString) errorString.append(tr("Vehicle is not flying. Smart RTL not available.")); return false; } + + return true; } bool WimaController::_calcReturnPath(QString &errorSring) @@ -807,15 +843,7 @@ bool WimaController::_calcReturnPath(QString &errorSring) } // 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(); + _saveCurrentMissionItemsToBuffer(); // create Mission Items removeFromVehicle(); @@ -904,9 +932,36 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow) _vehicleHasLowBattery = batteryLow; emit vehicleHasLowBatteryChanged(); - qWarning() << "WimaController::_setVehicleHasLowBattery(bool batteryLow)" << _vehicleHasLowBattery; } } +bool WimaController::_executeSmartRTL(QString &errorSring) +{ + Q_UNUSED(errorSring); + _executingSmartRTL = true; + connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp); + forceUploadToVehicle(); + masterController()->managerVehicle()->startMission(); + + return true; +} + +void WimaController::_loadCurrentMissionItemsFromBuffer() +{ + _currentMissionItems.clear(); + int numItems = _missionItemsBuffer.count(); + for (int i = 0; i < numItems; i++) + _currentMissionItems.append(_missionItemsBuffer.removeAt(0)); +} + + +void WimaController::_saveCurrentMissionItemsToBuffer() +{ + _missionItemsBuffer.clear(); + int numCurrentMissionItems = _currentMissionItems.count(); + for (int i = 0; i < numCurrentMissionItems; i++) + _missionItemsBuffer.append(_currentMissionItems.removeAt(0)); +} + diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 4dcbe2c363c969f48a4e79c8ada6954bff70d9bb..7f94f8952e52bed05c466edaddffc8c47f4d414a 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -102,7 +102,9 @@ public: Q_INVOKABLE bool forceUploadToVehicle(); 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 calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)# + Q_INVOKABLE bool executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring) + Q_INVOKABLE bool initSmartRTL(); Q_INVOKABLE void saveToCurrent (); @@ -168,6 +170,7 @@ private slots: void updateSpeed (void); void updateAltitude (void); void checkBatteryLevel (void); + void smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL private: void _setPhaseDistance(double distance); @@ -175,10 +178,9 @@ 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(); private: PlanMasterController *_masterController; @@ -224,6 +226,9 @@ private: QTimer _checkBatteryTimer; bool _vehicleHasLowBattery; bool _lowBatteryHandlingTriggered; + bool _executingSmartRTL; + + }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index e8f979fa7cf9f96dbca4c486b69b102cc7106ef5..beac49f86cec440f2725a61fb40dbea4b5666140 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -35,6 +35,11 @@ WimaPlaner::WimaPlaner(QObject *parent) _updateTimer.setInterval(500); // 250 ms means: max update time 2*250 ms _updateTimer.start(); + + // for debugging and testing purpose + connect(&_autoLoadTimer, &QTimer::timeout, this, &WimaPlaner::autoLoadMission); + _autoLoadTimer.setSingleShot(true); + _autoLoadTimer.start(300); } QmlObjectListModel* WimaPlaner::visualItems() @@ -381,8 +386,7 @@ bool WimaPlaner::loadFromFile(const QString &filename) errorString += QString(tr("Invalid or non existing entry for %s.\n").arg(WimaArea::areaTypeName)); return false; } - } - recalcJoinedArea(); + } _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension); @@ -442,7 +446,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) } } - //if (_circularSurvey == nullptr) + //if (_circularSurvey == nullptr) + recalcJoinedArea(); updateMission(); // remove temporary file @@ -808,6 +813,12 @@ void WimaPlaner::setSyncronizedWithControllerFalse() setSyncronizedWithController(false); } +void WimaPlaner::autoLoadMission() +{ + loadFromFile("/home/valentin/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release/345.wima"); + pushToContainer(); +} + QJsonDocument WimaPlaner::saveToJson(FileType fileType) { /// This function save all areas (of WimaPlaner) and all mission items (of MissionController) to a QJsonDocument diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 4b04fd5b6b539a77ae5e2ca2842490e78a6f0e03..c07c8c1bfad5cca67c87107005d410ebb6ac1824 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -133,6 +133,7 @@ private slots: // called by _updateTimer::timeout signal, updates different mission parts, if parameters (e.g. survey or areas) have changed void updateTimerSlot (); void setSyncronizedWithControllerFalse (void); + void autoLoadMission (void); private: signals: void joinedAreaValidChanged(); @@ -174,4 +175,6 @@ private: // sync stuff bool _syncronizedWithController; // true if planData is syncronized with wimaController bool _readyForSync; // gets set by updateMission and calcArrivalAndReturnPath + + QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents seg. faults };