diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 47f87b22a28f33a3883ee737f501243d8c13f2f5..4b396dd03d8c9787f3f02c424ea8055da2645444 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -443,9 +443,11 @@ HEADERS += \ src/Wima/WaypointManager/GenericWaypointManager.h \ src/Wima/Geometry/WimaPolygonArray.h \ src/Wima/Snake/snaketile.h \ + src/Wima/WaypointManager/RTLManager.h \ src/Wima/WaypointManager/Settings.h \ src/Wima/WaypointManager/Slicer.h \ src/Wima/WaypointManager/Utils.h \ + src/Wima/WimaBridge.h \ src/api/QGCCorePlugin.h \ src/api/QGCOptions.h \ src/api/QGCSettings.h \ @@ -456,7 +458,6 @@ HEADERS += \ src/Wima/Geometry/WimaServiceArea.h \ src/Wima/Geometry/WimaTrackerPolyline.h \ src/Wima/WimaController.h \ - src/Wima/WimaDataContainer.h \ src/Wima/WimaPlaner.h \ src/Wima/Geometry/WimaMeasurementArea.h \ src/Wima/Geometry/WimaCorridor.h \ @@ -502,9 +503,11 @@ SOURCES += \ src/Wima/WaypointManager/AreaInterface.cpp \ src/Wima/WaypointManager/DefaultManager.cpp \ src/Wima/WaypointManager/GenericWaypointManager.cpp \ + src/Wima/WaypointManager/RTLManager.cpp \ src/Wima/WaypointManager/Settings.cpp \ src/Wima/WaypointManager/Slicer.cpp \ src/Wima/WaypointManager/Utils.cpp \ + src/Wima/WimaBridge.cc \ src/comm/ros_bridge/include/ComPrivateInclude.cpp \ src/comm/ros_bridge/include/MessageTag.cpp \ src/comm/ros_bridge/include/TopicPublisher.cpp \ @@ -520,7 +523,6 @@ SOURCES += \ src/Wima/Geometry/WimaServiceArea.cc \ src/Wima/Geometry/WimaTrackerPolyline.cc \ src/Wima/WimaController.cc \ - src/Wima/WimaDataContainer.cc \ src/Wima/WimaPlaner.cc \ src/Wima/Geometry/WimaMeasurementArea.cc \ src/Wima/Geometry/WimaCorridor.cc \ diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 0b7599e94a869c92b50c8cb623ff4d66ff97be05..c125fe43b20bcfd15c0f8142967a4d63b64aed77 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -118,15 +118,22 @@ QGCView { WimaController { id: wimaController Component.onCompleted: { - wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer }) + wBridge.wimaController = Qt.binding(function() { return wimaController.thisPointer() }) } - onReturnUserRequestConfirmRequired: { - _guidedController.confirmAction(_guidedController.actionUploadAndExecute) + onForceUploadConfirm: { + _guidedController.confirmAction( + _guidedController.actionForceUpload) } - onReturnBatteryLowConfirmRequired: { - _guidedController.confirmAction(_guidedController.actionReturnBatteryLow) + onSmartRTLRequestConfirm: { + _guidedController.confirmAction( + _guidedController.actionSmartRTLRequestConfirm) + } + + onSmartRTLPathConfirm: { + _guidedController.confirmAction( + _guidedController.actionSmartRTLPathConfirm) } } @@ -765,14 +772,6 @@ QGCView { altitudeSlider: _altitudeSlider z: _flightVideoPipControl.z + 1 - property bool uploadOverrideRequired: wimaController.uploadOverrideRequired - - onUploadOverrideRequiredChanged: { - if (uploadOverrideRequired) { - confirmAction(actionOverrideUpload) - } - } - onShowStartMissionChanged: { if (showStartMission) { confirmAction(actionStartMission) diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 35c1333e4c1f011812c5b57f3205e0991d0c9da7..3842a39140477cf2a820333a724505599f1058e0 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -354,7 +354,7 @@ Item { text: qsTr("Upload") onClicked: { if (!planMasterController.offline) { - wimaController.uploadToVehicle() + wimaController.upload() } } Layout.fillWidth: true diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 0436473e9d9982fd5a3542c63a25ad052a30d0cc..5263e2502566de658baf7eb3cab9bee54356a47a 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -52,9 +52,9 @@ Item { readonly property string setWaypointTitle: qsTr("Set Waypoint") readonly property string gotoTitle: qsTr("Goto Location") readonly property string vtolTransitionTitle: qsTr("VTOL Transition") - readonly property string overrideUploadTitle: qsTr("Override Lock") - readonly property string uploadAndExecuteTitle: qsTr("Upload and Execute") - readonly property string returnBatteryLowTitle: qsTr("Battery Low") + readonly property string forceUploadTitle: qsTr("Force Upload") + readonly property string smartRTLPathConfirmTitle: qsTr("Upload and Execute") + readonly property string smartRTLRequestConfirmTitle: qsTr("Smart RTL requested") readonly property string armMessage: qsTr("Arm the vehicle.") readonly property string disarmMessage: qsTr("Disarm the vehicle") @@ -74,9 +74,9 @@ 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 uploadAndExecuteMessage: qsTr("Upload and execute the displayed return path?") - readonly property string returnBatteryLowMessage: qsTr("Upload and execute the displayed return path?") + readonly property string forceUploadMessage: qsTr("Vehicle outside save area. Force upload?") + readonly property string smartRTLPathConfirmMessage: qsTr("Upload and execute the displayed return path?") + readonly property string smartRTLRequestConfirmMessage: qsTr("Do you want to initiate a smart RTL?") readonly property int actionRTL: 1 @@ -100,9 +100,9 @@ Item { readonly property int actionMVStartMission: 19 readonly property int actionVtolTransitionToFwdFlight: 20 readonly property int actionVtolTransitionToMRFlight: 21 - readonly property int actionOverrideUpload: 22 - readonly property int actionUploadAndExecute: 24 - readonly property int actionReturnBatteryLow: 25 // very similar to actionUploadAndExecute, is triggered on low battery + readonly property int actionForceUpload: 22 + readonly property int actionSmartRTLPathConfirm: 24 + readonly property int actionSmartRTLRequestConfirm: 25 // very similar to actionSmartRTLPathConfirm, is triggered on low battery property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying property bool showArm: _guidedActionsEnabled && !_vehicleArmed @@ -343,19 +343,19 @@ Item { confirmDialog.message = vtolTransitionMRMessage confirmDialog.hideTrigger = true break - case actionOverrideUpload: - confirmDialog.title = overrideUploadTitle - confirmDialog.message = overrideUploadMessage + case actionForceUpload: + confirmDialog.title = forceUploadTitle + confirmDialog.message = forceUploadMessage confirmDialog.hideTrigger = true break - case actionUploadAndExecute: - confirmDialog.title = uploadAndExecuteTitle - confirmDialog.message = uploadAndExecuteMessage + case actionSmartRTLPathConfirm: + confirmDialog.title = smartRTLPathConfirmTitle + confirmDialog.message = smartRTLPathConfirmMessage confirmDialog.hideTrigger = true break - case actionReturnBatteryLow: - confirmDialog.title = returnBatteryLowTitle - confirmDialog.message = returnBatteryLowMessage + case actionSmartRTLRequestConfirm: + confirmDialog.title = smartRTLRequestConfirmTitle + confirmDialog.message = smartRTLRequestConfirmMessage confirmDialog.hideTrigger = true break default: @@ -433,13 +433,15 @@ Item { case actionVtolTransitionToMRFlight: _activeVehicle.vtolInFwdFlight = false break - case actionOverrideUpload: - wimaController.forceUploadToVehicle() + case actionForceUpload: + wimaController.forceUpload() break - case actionUploadAndExecute: - case actionReturnBatteryLow: + case actionSmartRTLPathConfirm: wimaController.executeSmartRTL() break + case actionSmartRTLRequestConfirm: + wimaController.initSmartRTL() + break default: console.warn(qsTr("Internal error: unknown actionCode"), actionCode) break diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index c70779885864ba1a4d783295444e4987523235c8..b6dd776207659f3f27b50df2f41d486f61bebfed 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -68,7 +68,7 @@ #include "CoordinateVector.h" #include "PlanMasterController.h" #include "Wima/WimaController.h" -#include "Wima/WimaDataContainer.h" +#include "Wima/WimaBridge.h" #include "Wima/WimaPlaner.h" #include "VideoManager.h" #include "VideoSurface.h" @@ -468,7 +468,7 @@ void QGCApplication::_initCommon(void) // Wima qmlRegisterType ("Wima", 1, 0, "WimaController"); qmlRegisterType ("Wima", 1, 0, "WimaPlaner"); - qmlRegisterType ("Wima", 1, 0, "WimaDataContainer"); + qmlRegisterType ("Wima", 1, 0, "WimaBridge"); // Register Qml Singletons diff --git a/src/Wima/WaypointManager/DefaultManager.cpp b/src/Wima/WaypointManager/DefaultManager.cpp index c9690b29f41060fc180d2cb90b2f2e0e12a52946..a7812c4af462398c7459c0203ff019d29a3b507b 100644 --- a/src/Wima/WaypointManager/DefaultManager.cpp +++ b/src/Wima/WaypointManager/DefaultManager.cpp @@ -108,6 +108,11 @@ bool WaypointManager::DefaultManager::_calcShortestPath( bool WaypointManager::DefaultManager::_worker() { + // Precondition: + // _waypoints must contain valid coordinates. + // Slicer must be called befor invoking this function. + // E.g. Slicer::reset(_waypoints, _currentWaypoints); + using namespace WaypointManager::Utils; if (_waypoints.count() < 1 || !_settings->valid()) { @@ -131,13 +136,6 @@ bool WaypointManager::DefaultManager::_worker() _currentMissionItems.clearAndDeleteContents(); initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView()); - // Precondition. - if (!_settings->homePosition().isValid()){ - qWarning("WaypointManager::DefaultManager::next(): home position invalid."); - Q_ASSERT(false); - return false; - } - // Calculate path from home to first waypoint. QVector arrivalPath; if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) { @@ -226,9 +224,7 @@ bool WaypointManager::DefaultManager::_worker() qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } - speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero) - speedItem->setCoordinate(_currentWaypoints.last()); - speedItem->missionItem().setParam2(_settings->arrivalReturnSpeed()); + makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed()); // Insert return path coordinates. for (auto coordinate : returnPath) { @@ -246,38 +242,37 @@ bool WaypointManager::DefaultManager::_worker() qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } + makeLandCmd(landItem, _settings->masterController()->managerVehicle()); - MAV_CMD landCmd = _settings->vehicle()->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; - if (_settings->vehicle()->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { - landItem->setCommand(landCmd); - } else { - Q_ASSERT(false); - qWarning("WaypointManager::DefaultManager::next(): Land command not supported!"); - return false; + // Set altitude. + for (int i = 1; i < _currentMissionItems.count(); ++i) { + SimpleMissionItem *item = _currentMissionItems.value(i); + if (item == nullptr) { + Q_ASSERT(false); + qWarning("WimaController::updateAltitude(): nullptr"); + return false; + } + item->altitude()->setRawValue(_settings->altitude()); } + // Update list _currentMissionItems. + updateHirarchy(_currentMissionItems); + updateSequenceNumbers(_currentMissionItems); + + // Prepend arrival path to slice. for ( long i = arrivalPath.size()-1; i >=0; --i ) _currentWaypoints.push_front(arrivalPath[i]); + _currentWaypoints.push_front(_settings->homePosition()); // Append return path to slice. for ( auto c : returnPath ) _currentWaypoints.push_back(c); + _currentWaypoints.push_back(_settings->homePosition()); + // Create variant list. _currentWaypointsVariant.clear(); for ( auto c : _currentWaypoints) _currentWaypointsVariant.push_back(QVariant::fromValue(c)); - - // Set altitude. - for (int i = 1; i < _currentMissionItems.count(); ++i) { - SimpleMissionItem *item = _currentMissionItems.value(i); - if (item == nullptr) { - Q_ASSERT(false); - qWarning("WimaController::updateAltitude(): nullptr"); - return false; - } - item->altitude()->setRawValue(_settings->altitude()); - } - return true; } diff --git a/src/Wima/WaypointManager/GenericWaypointManager.h b/src/Wima/WaypointManager/GenericWaypointManager.h index 54a126ac6d69d59fb5824cfd959853d37575e466..e50fc8407d9bcf6b49d2318c5f504d250010863a 100644 --- a/src/Wima/WaypointManager/GenericWaypointManager.h +++ b/src/Wima/WaypointManager/GenericWaypointManager.h @@ -19,13 +19,13 @@ public: GenericWaypointManager(SettingsType &settings); // Waypoint editing. - void setWaypoints(const WaypointList &waypoints); - void push_back (const WaypointType &wp); - void push_front (const WaypointType &wp); - virtual void clear (); - void insert (std::size_t i, const WaypointType &wp); - std::size_t size () const; - WaypointType &at (std::size_t i); + virtual void setWaypoints(const WaypointList &waypoints); + virtual void push_back (const WaypointType &wp); + virtual void push_front (const WaypointType &wp); + virtual void clear (); + virtual void insert (std::size_t i, const WaypointType &wp); + virtual std::size_t size () const; + virtual void remove (std::size_t i); const WaypointList &waypoints() const; @@ -141,12 +141,12 @@ template class ContainerType, class MissionItemList, class SettingsType> -WaypointType & GenericWaypointManager::at(std::size_t i) +void GenericWaypointManager::remove(std::size_t i) { - return _waypoints.at(i); + return _waypoints.remove(i); } template &waypoints) +{ + (void)waypoints; + return; +} + +void WaypointManager::RTLManager::push_back(const QGeoCoordinate &wp) +{ + (void)wp; + return; +} + +void WaypointManager::RTLManager::push_front(const QGeoCoordinate &wp) +{ + (void)wp; + return; +} + +void WaypointManager::RTLManager::clear() +{ + _dirty = true; + _waypoints.clear(); + _currentWaypoints.clear(); + _missionItems.clearAndDeleteContents(); + _currentMissionItems.clearAndDeleteContents(); + _waypointsVariant.clear(); + _currentWaypointsVariant.clear(); +} + +void WaypointManager::RTLManager::insert(std::size_t i, const QGeoCoordinate &wp) +{ + (void)i; + (void)wp; + return; +} + +std::size_t WaypointManager::RTLManager::size() const +{ + return 0; +} + +void WaypointManager::RTLManager::remove(std::size_t i) +{ + (void)i; +} + +bool WaypointManager::RTLManager::update() +{ + this->clear(); + return _worker(); +} + +bool WaypointManager::RTLManager::next() +{ + return true; +} + +bool WaypointManager::RTLManager::previous() +{ + return true; +} + +bool WaypointManager::RTLManager::reset() +{ + return true; +} + +QString WaypointManager::RTLManager::checkPrecondition() +{ + Vehicle *managerVehicle = _settings->masterController()->managerVehicle(); + if (!managerVehicle->flying()) { + return QString("Vehicle is not flying. Smart RTL not available."); + } + + if (!managerVehicle->isOfflineEditingVehicle()) { + return QString("Not connected to any vehicle. Smart RTL not available."); + } + + if (!_areaInterface->joinedArea()->containsCoordinate(managerVehicle->coordinate())) { + return QString("Vehicle not inside save area. Smart RTL not available."); + } + + return QString(); +} + +bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c, + size_t index, + QmlObjectListModel &list, + bool doUpdate) +{ + using namespace WaypointManager::Utils; + + if ( !insertMissionItem(c, + index /*insertion index*/, + list, + _settings->vehicle(), + _settings->isFlyView(), + &list /*parent*/, + doUpdate /*do update*/) ) + { + qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed."); + Q_ASSERT(false); + return false; + } + return true; +} + +bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c, + size_t index, + bool doUpdate) +{ + return _insertMissionItem(c, index, _currentMissionItems, doUpdate); +} + +bool WaypointManager::RTLManager::_calcShortestPath( + const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path) +{ + using namespace GeoUtilities; + using namespace PolygonCalculus; + QPolygonF joinedArea2D; + toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D); + QPointF start2D(0,0); + QPointF end2D; + toCartesian(destination, start, end2D); + QVector path2DOut; + + bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut); + toGeoList(path2DOut, /*origin*/ start, path); + + return retVal; +} + +bool WaypointManager::RTLManager::_worker() +{ + // Precondition: settings must be valid. + + using namespace WaypointManager::Utils; + + if (!_settings->valid()) { + return false; + } + + initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView()); + + // Calculate path from vehicle to home position. + QVector returnPath; + auto vehicleCoordinate = _settings->masterController()->managerVehicle()->coordinate(); + if ( !_calcShortestPath(vehicleCoordinate, _settings->homePosition(), returnPath) ) { + qWarning("WaypointManager::RTLManager::next(): Not able to calc path from vehicle to home position."); + return false; + } + + // Create mission items. + // Set home position of MissionSettingsItem. + MissionSettingsItem* settingsItem = _currentMissionItems.value(0); + if (settingsItem == nullptr) { + Q_ASSERT(false); + qWarning("WaypointManager::RTLManager::next(): nullptr."); + return false; + } + settingsItem->setCoordinate(_settings->homePosition()); + + // Create change speed item. + _insertMissionItem(_settings->homePosition(), + _currentMissionItems.count() /*insertion index*/, + false /*do update*/); + SimpleMissionItem *speedItem = _currentMissionItems.value(2); + if (speedItem == nullptr) { + qWarning("WaypointManager::RTLManager::next(): nullptr."); + Q_ASSERT(speedItem != nullptr); + return false; + } + makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed()); + + // Insert return path coordinates. + for (auto coordinate : returnPath) { + _insertMissionItem(coordinate, + _currentMissionItems.count() /*insertion index*/, + false /*do update*/); + } + + // Set land command for last mission item. + int index = _currentMissionItems.count(); + _insertMissionItem(_settings->homePosition(), + index /*insertion index*/, + false /*do update*/); + SimpleMissionItem *landItem = _currentMissionItems.value(index); + if (landItem == nullptr) { + Q_ASSERT(false); + qWarning("WaypointManager::RTLManager::next(): nullptr."); + return false; + } + makeLandCmd(landItem, _settings->masterController()->managerVehicle()); + + + // Set altitude. + for (int i = 1; i < _currentMissionItems.count(); ++i) { + SimpleMissionItem *item = _currentMissionItems.value(i); + if (item == nullptr) { + Q_ASSERT(false); + qWarning("WimaController::updateAltitude(): nullptr"); + return false; + } + item->altitude()->setRawValue(_settings->altitude()); + } + + // Update list _currentMissionItems. + updateHirarchy(_currentMissionItems); + updateSequenceNumbers(_currentMissionItems); + + + // Append return path to _currentWaypoints. + for ( auto c : returnPath ) + _currentWaypoints.push_back(c); + + // Create variant list. + _currentWaypointsVariant.clear(); + for ( auto c : _currentWaypoints) + _currentWaypointsVariant.push_back(QVariant::fromValue(c)); + + return true; +} diff --git a/src/Wima/WaypointManager/RTLManager.h b/src/Wima/WaypointManager/RTLManager.h new file mode 100644 index 0000000000000000000000000000000000000000..9166beaf89919df06ae0d9657b2b8cbc3fd9a8a2 --- /dev/null +++ b/src/Wima/WaypointManager/RTLManager.h @@ -0,0 +1,99 @@ +#pragma once + +#include "GenericWaypointManager.h" +#include "Settings.h" +#include "AreaInterface.h" + +namespace WaypointManager { + +typedef GenericWaypointManager + ManagerBase; +//! +//! \brief The RTLManager class is used to manage the return to launch of the vehicle. +//! +//! @note The \class QmlObjectList returned by \fn missionItems doesn't contain a MissionSettingsItem. +//! This list is supposed to be used for display purposes only. +class RTLManager : public ManagerBase +{ +public: + RTLManager() = delete; + RTLManager(Settings &settings, + AreaInterface &interface); + + //! + //! \brief Does nothing. + //! + virtual void setWaypoints(const QVector &waypoints) override; + //! + //! \brief Does nothing. + //! + virtual void push_back (const QGeoCoordinate &wp) override; + //! + //! \brief Does nothing. + //! + virtual void push_front (const QGeoCoordinate &wp) override; + //! + //! \brief Clears the waypoint manager. + virtual void clear () override; + //! + //! \brief Does nothing. + //! + virtual void insert (std::size_t i, const QGeoCoordinate &wp) override; + //! + //! \brief Returns zero. + //! \return Returns zero. + virtual std::size_t size () const override; + //! + //! \brief Returns an empty coordinate. + virtual void remove (std::size_t i) override; + + + //! + //! \brief Updates the waypoint manager. + //! + //! After updateing a call to currentMissionItems or currentWaypoints returns + //! a path from the vehicles current position to the home position. + //! + //! \return Returns true on success, false either. + //! + virtual bool update() override; + //! + //! \brief Does nothing. + //! \return Returns true. + //! + virtual bool next() override; + //! + //! \brief Does nothing. + //! \return Returns true. + //! + virtual bool previous() override; + //! + //! \brief Does nothing. + //! \return Returns true. + //! + virtual bool reset() override; + + QString checkPrecondition(); + +protected: + bool _insertMissionItem(const QGeoCoordinate &c, + size_t index, + QmlObjectListModel &list, + bool doUpdate); + bool _insertMissionItem(const QGeoCoordinate &c, + size_t index, + bool doUpdate); + bool _calcShortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path); + + AreaInterface *_areaInterface; +private: + bool _worker(); + +}; + +} // namespace WaypointManager diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp index 7792e12b3a4a6ef2f757891f7caa41e43e52196f..a880f52258536244fffc6e362c7344e1d0754dcf 100644 --- a/src/Wima/WaypointManager/Slicer.cpp +++ b/src/Wima/WaypointManager/Slicer.cpp @@ -1,25 +1,26 @@ #include "Slicer.h" -Slicer::Slicer(): - _idxValid(false) - , _atEnd(false) +Slicer::Slicer() + : _idxStart(0) + , _idxEnd(0) + , _idxNext(0) + , _idxPrevious(0) + , _overlap(0) + , _N(0) {} void Slicer::setOverlap(uint32_t overlap) { - _idxValid = false; _overlap = overlap; } void Slicer::setN(uint32_t N) { - _idxValid = false; _N = N > 0 ? N : 1; } void Slicer::setStartIndex(int idxStart) { - _idxValid = false; _idxStart = idxStart; } @@ -38,28 +39,30 @@ int Slicer::startIndex() return _idxStart; } -void Slicer::_updateIdx(std::size_t size) +void Slicer::_updateIdx(long size) { - _idxValid = true; - _atEnd = false; - if ( _idxStart >= long(size)-1 ) { - _idxStart = long(size)-1; - _idxEnd = _idxStart; - _idxNext = _idxStart; - _atEnd = true; - return; - } + _overlap = _overlap < _N ? _overlap : _N-1; - _idxStart = _idxStart < 0 ? 0 : _idxStart; + long maxStart = size-_N; + _idxStart = _idxStart <= maxStart ? _idxStart : maxStart; + _idxStart = _idxStart < 0 ? 0 : _idxStart; _idxEnd = _idxStart + _N - 1; - _idxEnd = _idxEnd < long(size) ? _idxEnd : size-1; + _idxEnd = _idxEnd < size ? _idxEnd : size-1; _idxNext = _idxEnd + 1 - _overlap; - _idxNext = _idxNext < 0 ? 0 : _idxNext; - _idxNext = _idxNext < long(size) ? _idxNext : size-1; + _idxNext = _idxNext < 0 ? 0 : _idxNext; + _idxNext = _idxNext < size ? _idxNext : size-1; - _idxPrevious = _idxStart - 1 + _overlap; - _idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious; - _idxPrevious = _idxPrevious < long(size) ? _idxPrevious : size-1; + _idxPrevious = _idxStart + _overlap - _N; + _idxPrevious = _idxPrevious < 0 ? 0 : _idxPrevious; + _idxPrevious = _idxPrevious < size ? _idxPrevious : size-1; + +// qDebug() << "size: " << size; +// qDebug() << "_N: " << _N; +// qDebug() << "_overlap: " << _overlap; +// qDebug() << "_idxStart: " << _idxStart; +// qDebug() << "_idxEnd: " << _idxEnd; +// qDebug() << "_idxNext: " << _idxNext; +// qDebug() << "_idxPrevious: " << _idxPrevious << "\n"; } diff --git a/src/Wima/WaypointManager/Slicer.h b/src/Wima/WaypointManager/Slicer.h index ae0c996fcdff3f7dddbba1de8c5512469fbf0a8a..354db75cd982d3775d9cb17385f487fc2e1a29df 100644 --- a/src/Wima/WaypointManager/Slicer.h +++ b/src/Wima/WaypointManager/Slicer.h @@ -48,17 +48,14 @@ public: private: - void _updateIdx(std::size_t size); + void _updateIdx(long size); long _idxStart; long _idxEnd; long _idxNext; long _idxPrevious; - uint32_t _overlap; - uint32_t _N; - bool _idxValid; - bool _atEnd; - + long _overlap; + long _N; }; diff --git a/src/Wima/WaypointManager/Utils.cpp b/src/Wima/WaypointManager/Utils.cpp index b75f962189e5fa59b14a3ee4dac9cc06965ba714..ce4c576054e9aa4f012183af156958f67ec46b74 100644 --- a/src/Wima/WaypointManager/Utils.cpp +++ b/src/Wima/WaypointManager/Utils.cpp @@ -218,3 +218,16 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) item->setCoordinate(c); item->missionItem().setParam2(speed); } + +bool WaypointManager::Utils::makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle) +{ + MAV_CMD landCmd = vehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; + if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { + item->setCommand(landCmd); + } else { + Q_ASSERT(false); + qWarning("WaypointManager::Utils::makeLandCmd(): Land command not supported!"); + return false; + } + return true; +} diff --git a/src/Wima/WaypointManager/Utils.h b/src/Wima/WaypointManager/Utils.h index be575ef332df9b9421dba29ecbbe90707f261c2b..2351f98740634d45897ae42439a0fb59dadb4d11 100644 --- a/src/Wima/WaypointManager/Utils.h +++ b/src/Wima/WaypointManager/Utils.h @@ -118,6 +118,14 @@ bool extract(const ContainerType &source, //! bool makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle); +//! +//! \brief Makes the SimpleMissionItem \p item a land command. +//! \param item SimpleMissionItem +//! \param vehilce Vehicle. +//! \return Returns true if successfull, false either. +//! +bool makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle); + //! //! \brief Makes the SimpleMissionItem \p item a MAV_CMD_DO_CHANGE_SPEED item. //! \param item SimpleMissionItem. diff --git a/src/Wima/WimaBridge.cc b/src/Wima/WimaBridge.cc new file mode 100644 index 0000000000000000000000000000000000000000..efc886f54856c1d513756ec53dccba335456b2ad --- /dev/null +++ b/src/Wima/WimaBridge.cc @@ -0,0 +1,54 @@ +#include "WimaBridge.h" +#include "WimaController.h" + +WimaBridge::WimaBridge(QObject *parent) + : QObject (parent) +{ + +} + +WimaController *WimaBridge::wimaController() +{ + return _wimaController; +} + +WimaPlaner *WimaBridge::wimaPlaner() +{ + return _wimaPlaner; +} + +WimaBridge *WimaBridge::thisPointer() +{ + return this; +} + +void WimaBridge::setWimaController(WimaController *controller) +{ + if (_wimaController != controller){ + _wimaController = controller; + + emit wimaControllerChanged(_wimaController); + } +} + +void WimaBridge::setWimaPlaner(WimaPlaner *planer) +{ + if (_wimaPlaner != planer){ + _wimaPlaner = planer; + + emit wimaPlanerChanged(_wimaPlaner); + } + +} + +bool WimaBridge::setWimaPlanData(const WimaPlanData &planData) +{ + if ( _wimaController != nullptr) { + return _wimaController->setWimaPlanData(planData); + } + return false; +} + + + + diff --git a/src/Wima/WimaBridge.h b/src/Wima/WimaBridge.h new file mode 100644 index 0000000000000000000000000000000000000000..1615f1b7163f7c8ae182f207f589ae5533040761 --- /dev/null +++ b/src/Wima/WimaBridge.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include "WimaPlanData.h" + +class WimaController; +class WimaPlaner; + +//! +//! \brief The WimaBridge class +//! +//! A bridge establishing a link between WimaController and WimaPlaner +class WimaBridge : public QObject +{ + Q_OBJECT +public: + WimaBridge(QObject *parent = nullptr); + WimaBridge(WimaBridge &other) = delete; + + Q_PROPERTY(WimaPlaner *wimaPlaner + READ wimaPlaner + WRITE setWimaPlaner + NOTIFY wimaPlanerChanged) + Q_PROPERTY(WimaController *wimaController + READ wimaController + WRITE setWimaController + NOTIFY wimaControllerChanged) + + WimaController *wimaController(); + WimaPlaner *wimaPlaner(); + + Q_INVOKABLE WimaBridge *thisPointer(); + + void setWimaController (WimaController *controller); + void setWimaPlaner (WimaPlaner *planer); + +signals: + void wimaControllerChanged (WimaController *controller); + void wimaPlanerChanged (WimaPlaner *planer); + +public slots: + bool setWimaPlanData(const WimaPlanData &planData); +private: + WimaController *_wimaController; + WimaPlaner *_wimaPlaner; +}; + diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index c33dc0594b9c15ded5ba1bec4e774b1797a57c6c..536c852378cd32e8d18a44828b57b89354a893a7 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -10,9 +10,6 @@ #include "Snake/QtROSTypeFactory.h" #include "Snake/QNemoProgress.h" -#include "time.h" -#include "assert.h" - #include "QVector3D" #include @@ -41,7 +38,6 @@ using namespace snake_geometry; WimaController::WimaController(QObject *parent) : QObject (parent) - , _container (nullptr) , _joinedArea () , _measurementArea () , _serviceArea () @@ -51,7 +47,8 @@ WimaController::WimaController(QObject *parent) , _managerSettings () , _defaultManager (_managerSettings, _areaInterface) , _snakeManager (_managerSettings, _areaInterface) - , _currentManager (_defaultManager) + , _rtlManager (_managerSettings, _areaInterface) + , _currentManager (&_defaultManager) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) @@ -62,17 +59,8 @@ WimaController::WimaController(QObject *parent) , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) , _altitude (settingsGroup, _metaDataMap[altitudeName]) - , _uploadOverrideRequired (false) , _measurementPathLength (-1) -// , _arrivalPathLength (-1) -// , _returnPathLength (-1) -// , _phaseDistance (-1) -// , _phaseDuration (-1) -// , _phaseDistanceBuffer (-1) -// , _phaseDurationBuffer (-1) - , _vehicleHasLowBattery (false) , _lowBatteryHandlingTriggered (false) - , _executingSmartRTL (false) , _snakeConnectionStatus (SnakeConnectionStatus::Connected) // TODO: implement automatic connection , _snakeCalcInProgress (false) , _scenarioDefinedBool (false) @@ -88,7 +76,7 @@ WimaController::WimaController(QObject *parent) _showCurrentMissionItems.setRawValue(true); connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap); connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints); - connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_setStartIndex); connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed); connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude); @@ -101,10 +89,6 @@ WimaController::WimaController(QObject *parent) connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); _eventTimer.setInterval(EVENT_TIMER_INTERVAL); - Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); - connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::_enableDisableLowBatteryHandling); - _enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue()); - // Snake Worker Thread. connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); @@ -112,8 +96,11 @@ WimaController::WimaController(QObject *parent) // Start, stop RosBridge. connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge); - connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); _startStopRosBridge(); + connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); + _initStartSnakeWorker(); + connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); + _switchSnakeManager(_enableSnake.rawValue()); } PlanMasterController *WimaController::masterController() { @@ -128,26 +115,22 @@ QmlObjectListModel *WimaController::visualItems() { return &_areas; } -WimaDataContainer *WimaController::dataContainer() { - return _container; -} - QmlObjectListModel *WimaController::missionItems() { - return const_cast(&_currentManager.missionItems()); + return const_cast(&_currentManager->missionItems()); } QmlObjectListModel *WimaController::currentMissionItems() { - return const_cast(&_currentManager.currentMissionItems()); + return const_cast(&_currentManager->currentMissionItems()); } QVariantList WimaController::waypointPath() { - return const_cast(_currentManager.waypointsVariant()); + return const_cast(_currentManager->waypointsVariant()); } QVariantList WimaController::currentWaypointPath() { - return const_cast(_currentManager.currentWaypointsVariant()); + return const_cast(_currentManager->currentWaypointsVariant()); } Fact *WimaController::enableWimaController() { @@ -186,28 +169,6 @@ Fact *WimaController::altitude() { return &_altitude; } -//QStringList WimaController::loadNameFilters() const -//{ -// QStringList filters; - -// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) << -// tr("All Files (*.*)"); -// return filters; -//} - -//QStringList WimaController::saveNameFilters() const -//{ -// QStringList filters; - -// filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension); -// return filters; -//} - -bool WimaController::uploadOverrideRequired() const -{ - return _uploadOverrideRequired; -} - double WimaController::phaseDistance() const { return 0.0; @@ -218,11 +179,6 @@ double WimaController::phaseDuration() const return 0.0; } -bool WimaController::vehicleHasLowBattery() const -{ - return _vehicleHasLowBattery; -} - long WimaController::snakeConnectionStatus() const { return _snakeConnectionStatus; @@ -247,35 +203,6 @@ void WimaController::setMissionController(MissionController *missionC) emit missionControllerChanged(); } -/*! - * \fn void WimaController::setDataContainer(WimaDataContainer *container) - * Sets the pointer to the \c WimaDataContainer, which is meant to exchange data between the \c WimaController and the \c WimaPlaner. - * - * \sa WimaPlaner, WimaDataContainer, WimaPlanData - */ -void WimaController::setDataContainer(WimaDataContainer *container) -{ - if (container != nullptr) { - if (_container != nullptr) { - disconnect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData); - } - - _container = container; - connect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData); - - emit dataContainerChanged(); - } -} - -void WimaController::setUploadOverrideRequired(bool overrideRequired) -{ - if (_uploadOverrideRequired != overrideRequired) { - _uploadOverrideRequired = overrideRequired; - - emit uploadOverrideRequiredChanged(); - } -} - void WimaController::nextPhase() { _calcNextPhase(); @@ -283,8 +210,8 @@ void WimaController::nextPhase() void WimaController::previousPhase() { - if ( !_currentManager.previous() ) { - assert(false); + if ( !_currentManager->previous() ) { + Q_ASSERT(false); } emit missionItemsChanged(); @@ -295,8 +222,8 @@ void WimaController::previousPhase() void WimaController::resetPhase() { - if ( !_currentManager.reset() ) { - assert(false); + if ( !_currentManager->reset() ) { + Q_ASSERT(false); } emit missionItemsChanged(); @@ -305,38 +232,42 @@ void WimaController::resetPhase() emit waypointPathChanged(); } -bool WimaController::uploadToVehicle() +void WimaController::requestSmartRTL() +{ + emit smartRTLRequestConfirm(); +} + +bool WimaController::upload() { auto ¤tMissionItems = _defaultManager.currentMissionItems(); if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate()) && currentMissionItems.count() > 0) { - setUploadOverrideRequired(true); + emit forceUploadConfirm(); return false; } - return forceUploadToVehicle(); + return forceUpload(); } -bool WimaController::forceUploadToVehicle() +bool WimaController::forceUpload() { auto ¤tMissionItems = _defaultManager.currentMissionItems(); - setUploadOverrideRequired(false); if (currentMissionItems.count() < 1) return false; _missionController->removeAll(); - // set homeposition of settingsItem + // Set homeposition of settingsItem. QmlObjectListModel* visuals = _missionController->visualItems(); MissionSettingsItem* settingsItem = visuals->value(0); if (settingsItem == nullptr) { - assert(false); + Q_ASSERT(false); qWarning("WimaController::updateCurrentMissionItems(): nullptr"); return false; } settingsItem->setCoordinate(_managerSettings.homePosition()); // Copy mission items to _missionController. - for (int i = 0; i < currentMissionItems.count(); i++){ + for (int i = 1; i < currentMissionItems.count(); i++){ auto *item = currentMissionItems.value(i); _missionController->insertSimpleMissionItem(*item, visuals->count()); } @@ -352,36 +283,14 @@ void WimaController::removeFromVehicle() _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::executeSmartRTL() { - _executeSmartRTL(); + forceUpload(); + masterController()->managerVehicle()->startMission(); } void WimaController::initSmartRTL() { - _srtlReason = UserRequest; _initSmartRTL(); } @@ -391,34 +300,7 @@ void WimaController::removeVehicleTrajectoryHistory() managerVehicle->trajectoryPoints()->clear(); } -//void WimaController::saveToFile(const QString& filename) -//{ -// QString file = filename; -//} - -//bool WimaController::loadFromCurrent() -//{ -// return true; -//} - -//bool WimaController::loadFromFile(const QString &filename) -//{ -// QString file = filename; -// return true; -//} - - - -//QJsonDocument WimaController::saveToJson(FileType fileType) -//{ -// if(fileType) -// { - -// } -// return QJsonDocument(); -//} - -bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) +bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { using namespace GeoUtilities; using namespace PolygonCalculus; @@ -442,7 +324,7 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo * * \sa WimaDataContainer, WimaPlaner, WimaPlanData */ -bool WimaController::_fetchContainerData() +bool WimaController::setWimaPlanData(const WimaPlanData &planData) { // fetch only if valid, return true on success @@ -463,14 +345,6 @@ bool WimaController::_fetchContainerData() _localPlanDataValid = false; - if (_container == nullptr) { - qWarning("WimaController::fetchContainerData(): No container assigned!"); - assert(false); - return false; - } - - WimaPlanData planData = _container->pull(); - // extract list with WimaAreas QList areaList = planData.areaList(); @@ -516,10 +390,12 @@ bool WimaController::_fetchContainerData() } if (areaCounter != numAreas) { - assert(false); + Q_ASSERT(false); return false; } + emit visualItemsChanged(); + // extract mission items QList tempMissionItems = planData.missionItems(); if (tempMissionItems.size() < 1) { @@ -536,10 +412,17 @@ bool WimaController::_fetchContainerData() 0) ); if( !_defaultManager.reset() ){ - assert(false); + Q_ASSERT(false); return false; } + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); + + + // Initialize _scenario. Area mArea; for (auto variant : _measurementArea.path()){ @@ -567,9 +450,10 @@ bool WimaController::_fetchContainerData() _scenario.addArea(corridor); // Check if scenario is defined. - if ( !_verifyScenarioDefinedWithErrorMessage() ) - assert(false); + if ( !_isScenarioDefinedErrorMessage() ) { + Q_ASSERT(false); return false; + } { // Get tiles and origin. @@ -607,36 +491,42 @@ bool WimaController::_fetchContainerData() _snakeTilesLocal.polygons().append(Tile); } } + emit snakeTilesChanged(); + emit snakeTileCenterPointsChanged(); + _localPlanDataValid = true; + return true; +} +WimaController *WimaController::thisPointer() +{ + return this; +} + +bool WimaController::_calcNextPhase() +{ + if ( !_currentManager->next() ) { + Q_ASSERT(false); + return false; + } - emit visualItemsChanged(); emit missionItemsChanged(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); - emit snakeTilesChanged(); - emit snakeTileCenterPointsChanged(); + emit waypointPathChanged(); - _localPlanDataValid = true; return true; } -bool WimaController::_calcNextPhase() +bool WimaController::_setStartIndex() { - qDebug() << "WimaController::_calcNextPhase: " - << "overlap=" << _currentManager.overlap() - << "N=" << _currentManager.N() - << "startIndex=" << _currentManager.startIndex(); -// bool value; -// _currentManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)); -// Q_ASSERT(value); -// (void)value; -// qDebug() << "overlap=" << _currentManager.overlap() -// << "N=" << _currentManager.N() -// << "startIndex=" << _currentManager.startIndex(); - - if ( !_currentManager.next() ) { - assert(false); + bool value; + _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); return false; } @@ -650,8 +540,8 @@ bool WimaController::_calcNextPhase() void WimaController::_recalcCurrentPhase() { - if ( !_currentManager.update() ) { - assert(false); + if ( !_currentManager->update() ) { + Q_ASSERT(false); } emit missionItemsChanged(); @@ -663,11 +553,11 @@ void WimaController::_recalcCurrentPhase() void WimaController::_updateOverlap() { bool value; - _currentManager.setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); + _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); Q_ASSERT(value); (void)value; - if ( !_currentManager.update() ) { + if ( !_currentManager->update() ) { assert(false); } @@ -680,25 +570,30 @@ void WimaController::_updateOverlap() void WimaController::_updateMaxWaypoints() { bool value; - _currentManager.setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); + _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); Q_ASSERT(value); (void)value; - if ( !_currentManager.update() ) { - assert(false); + if ( !_currentManager->update() ) { + Q_ASSERT(false); } emit missionItemsChanged(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); emit waypointPathChanged(); - } void WimaController::_updateflightSpeed() { + bool value; _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble()); - _currentManager.update(); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } emit missionItemsChanged(); emit currentMissionItemsChanged(); @@ -708,8 +603,14 @@ void WimaController::_updateflightSpeed() void WimaController::_updateArrivalReturnSpeed() { + bool value; _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble()); - _currentManager.update(); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } emit missionItemsChanged(); emit currentMissionItemsChanged(); @@ -719,8 +620,14 @@ void WimaController::_updateArrivalReturnSpeed() void WimaController::_updateAltitude() { + bool value; _managerSettings.setAltitude(_altitude.rawValue().toDouble()); - _currentManager.update(); + Q_ASSERT(value); + (void)value; + + if ( !_currentManager->update() ) { + Q_ASSERT(false); + } emit missionItemsChanged(); emit currentMissionItemsChanged(); @@ -743,21 +650,14 @@ void WimaController::_checkBatteryLevel() if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) { - _setVehicleHasLowBattery(true); - if (!_lowBatteryHandlingTriggered) { - - if (_missionController->remainingTime() <= minTime) { - _lowBatteryHandlingTriggered = true; - } - else { - _lowBatteryHandlingTriggered = true; - _srtlReason = BatteryLow; + if (!_lowBatteryHandlingTriggered) { + _lowBatteryHandlingTriggered = true; + if ( !(_missionController->remainingTime() <= minTime) ) { _initSmartRTL(); } } } else { - _setVehicleHasLowBattery(false); _lowBatteryHandlingTriggered = false; } @@ -771,7 +671,8 @@ void WimaController::_eventTimerHandler() static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000); // Battery level check necessary? - if ( batteryLevelTicker.ready() ) + Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); + if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() ) _checkBatteryLevel(); // Snake flight plan update necessary? @@ -796,28 +697,11 @@ void WimaController::_eventTimerHandler() void WimaController::_smartRTLCleanUp(bool flying) { - (void)flying; - -// if ( !flying) { // vehicle has landed -// if (_executingSmartRTL) { -// _executingSmartRTL = false; -// _loadCurrentMissionItemsFromBuffer(); -// _setPhaseDistance(_phaseDistanceBuffer); -// _setPhaseDuration(_phaseDurationBuffer); -// _showAllMissionItems.setRawValue(true); -// _missionController->removeAllFromVehicle(); -// _missionController->removeAll(); -// disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); -// } -// } -} - -void WimaController::_enableDisableLowBatteryHandling(QVariant enable) -{ - if (enable.toBool()) { - _eventTimer.start(); - } else { - _eventTimer.stop(); + if ( !flying) { // vehicle has landed + _switchWaypointManager(_defaultManager); + _missionController->removeAllFromVehicle(); + _missionController->removeAll(); + disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); } } @@ -841,73 +725,52 @@ void WimaController::_setPhaseDuration(double duration) // } } -bool WimaController::_checkSmartRTLPreCondition(QString &errorString) +QString WimaController::_checkSmartRTLPreCondition(void) { 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; - } - - if (!_joinedArea.containsCoordinate(managerVehicle->coordinate())) { - errorString.append(tr("Vehicle not inside save area. Smart RTL not available.")); - return false; + return QString(tr("No WiMA data available. Please define at least a measurement and a service area.")); } + auto errorString = _rtlManager.checkPrecondition(); - return true; + return errorString; } -void WimaController::_setVehicleHasLowBattery(bool batteryLow) +void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager) { - if (_vehicleHasLowBattery != batteryLow) { - _vehicleHasLowBattery = batteryLow; + if (_currentManager != &manager) { + _currentManager = &manager; + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); - emit vehicleHasLowBatteryChanged(); + qWarning() << "WimaController::_switchWaypointManager: statistics update missing."; } } 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; -// } - -// 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() -{ - forceUploadToVehicle(); - masterController()->managerVehicle()->startMission(); + QString errorString; + static int attemptCounter = 0; + attemptCounter++; + + if (!_checkSmartRTLPreCondition().isEmpty()) { + _masterController->managerVehicle()->pauseVehicle(); + connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); + if ( _rtlManager.update() ) { // Calculate return path. + _switchWaypointManager(_rtlManager); + attemptCounter = 0; + emit smartRTLPathConfirm(); + return; + } + } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { + errorString.append(tr("Smart RTL: No success after maximum number of attempts.")); + qgcApp()->showMessage(errorString); + attemptCounter = 0; + } else { + _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL); + } } void WimaController::_setSnakeConnectionStatus(WimaController::SnakeConnectionStatus status) @@ -926,15 +789,17 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress) } } -bool WimaController::_verifyScenarioDefined() +bool WimaController::_isScenarioDefined() { - _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(), _snakeTileHeight.rawValue().toDouble(), _snakeMinTileArea.rawValue().toDouble()); + _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(), + _snakeTileHeight.rawValue().toDouble(), + _snakeMinTileArea.rawValue().toDouble()); return _scenarioDefinedBool; } -bool WimaController::_verifyScenarioDefinedWithErrorMessage() +bool WimaController::_isScenarioDefinedErrorMessage() { - bool value = _verifyScenarioDefined(); + bool value = _isScenarioDefined(); if (!value){ QString errorString; for (auto c : _scenario.errorString) @@ -958,7 +823,10 @@ void WimaController::_snakeStoreWorkerResults() // create Mission items from r.waypoints long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2; - assert(n >= 1); + if (n >= 1) { + Q_ASSERT(n >= 1); + return; + } // Create QVector containing all waypoints; unsigned long startIdx = r.arrivalPathIdx.last(); @@ -1009,6 +877,15 @@ void WimaController::_initStartSnakeWorker() _snakeWorker.start(); } +void WimaController::_switchSnakeManager(QVariant variant) +{ + if (variant.value()){ + _switchWaypointManager(_snakeManager); + } else { + _switchWaypointManager(_defaultManager); + } +} + void WimaController::_progressFromJson(JsonDocUPtr pDoc, QNemoProgress &progress) { diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index bf603b0438735d3b9b51a2013cdaab42f196bb4f..e1c49ae2d12fdfda19849aaa3db3399e7e68f4eb 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -10,11 +10,12 @@ #include "Geometry/WimaMeasurementArea.h" #include "Geometry/WimaServiceArea.h" #include "Geometry/WimaCorridor.h" -#include "WimaDataContainer.h" #include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaCorridorData.h" #include "Geometry/WimaServiceAreaData.h" +#include "WimaPlanData.h" + #include "PlanMasterController.h" #include "MissionController.h" #include "SurveyComplexItem.h" @@ -36,6 +37,7 @@ #include "ros_bridge/include/ROSBridge.h" #include "WaypointManager/DefaultManager.h" +#include "WaypointManager/RTLManager.h" #define CHECK_BATTERY_INTERVAL 1000 // ms #define SMART_RTL_MAX_ATTEMPTS 3 // times @@ -63,7 +65,7 @@ public: WimaController(QObject *parent = nullptr); - + // Controllers. Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController @@ -74,19 +76,11 @@ public: WRITE setMissionController NOTIFY missionControllerChanged ) + // Wima Data. Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged ) -// Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged) -// Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) -// Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) -// Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) - Q_PROPERTY(WimaDataContainer* dataContainer - READ dataContainer - WRITE setDataContainer - NOTIFY dataContainerChanged - ) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged @@ -106,6 +100,7 @@ public: Q_PROPERTY(Fact* enableWimaController READ enableWimaController CONSTANT) + // Waypoint navigaton. Q_PROPERTY(Fact* overlapWaypoints READ overlapWaypoints CONSTANT @@ -126,6 +121,7 @@ public: READ showCurrentMissionItems CONSTANT ) + // Waypoint settings. Q_PROPERTY(Fact* flightSpeed READ flightSpeed CONSTANT @@ -138,11 +134,7 @@ public: READ arrivalReturnSpeed CONSTANT ) - Q_PROPERTY(bool uploadOverrideRequired - READ uploadOverrideRequired - WRITE setUploadOverrideRequired - NOTIFY uploadOverrideRequiredChanged - ) + // Waypoint statistics. Q_PROPERTY(double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged @@ -151,10 +143,6 @@ public: READ phaseDuration NOTIFY phaseDurationChanged ) - Q_PROPERTY(bool vehicleHasLowBattery - READ vehicleHasLowBattery - NOTIFY vehicleHasLowBatteryChanged - ) // Snake Q_PROPERTY(Fact* enableSnake @@ -205,19 +193,18 @@ public: // Property accessors + // Controllers. PlanMasterController* masterController (void); MissionController* missionController (void); + // Wima Data QmlObjectListModel* visualItems (void); -// QString currentFile (void) const { return _currentFile; } -// QStringList loadNameFilters (void) const; -// QStringList saveNameFilters (void) const; -// QString fileExtension (void) const { return wimaFileExtension; } QGCMapPolygon joinedArea (void) const; - WimaDataContainer* dataContainer (void); + // Waypoints. QmlObjectListModel* missionItems (void); QmlObjectListModel* currentMissionItems (void); QVariantList waypointPath (void); QVariantList currentWaypointPath (void); + // Settings facts. Fact* enableWimaController (void); Fact* overlapWaypoints (void); Fact* maxWaypointsPerPhase (void); @@ -227,54 +214,51 @@ public: Fact* flightSpeed (void); Fact* arrivalReturnSpeed (void); Fact* altitude (void); - + // Snake settings facts. Fact* enableSnake (void) { return &_enableSnake; } Fact* snakeTileWidth (void) { return &_snakeTileWidth;} Fact* snakeTileHeight (void) { return &_snakeTileHeight;} Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;} Fact* snakeLineDistance (void) { return &_snakeLineDistance;} Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;} + // Snake data. QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();} QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;} - QVector nemoProgress (void) { return _nemoProgress.progress();} + QVector nemoProgress (void) { return _nemoProgress.progress();} + long snakeConnectionStatus (void) const; + bool snakeCalcInProgress (void) const; + // Smart RTL. bool uploadOverrideRequired (void) const; + bool vehicleHasLowBattery (void) const; + // Waypoint statistics. double phaseDistance (void) const; double phaseDuration (void) const; - bool vehicleHasLowBattery (void) const; - - long snakeConnectionStatus (void) const; - bool snakeCalcInProgress (void) const; // Property setters void setMasterController (PlanMasterController* masterController); void setMissionController (MissionController* missionController); - void setDataContainer (WimaDataContainer* container); - void setUploadOverrideRequired (bool overrideRequired); + bool setWimaPlanData (const WimaPlanData &planData); // Member Methodes + Q_INVOKABLE WimaController *thisPointer(); + // Waypoint navigation. Q_INVOKABLE void nextPhase(); Q_INVOKABLE void previousPhase(); Q_INVOKABLE void resetPhase(); - Q_INVOKABLE bool uploadToVehicle(); - Q_INVOKABLE bool forceUploadToVehicle(); - Q_INVOKABLE void removeFromVehicle(); - Q_INVOKABLE bool checkSmartRTLPreCondition(); - Q_INVOKABLE bool calcReturnPath(); - Q_INVOKABLE void executeSmartRTL(); + // Smart RTL. + Q_INVOKABLE void requestSmartRTL(); Q_INVOKABLE void initSmartRTL(); + Q_INVOKABLE void executeSmartRTL(); + // Other. Q_INVOKABLE void removeVehicleTrajectoryHistory(); - - -// Q_INVOKABLE void saveToCurrent (); -// Q_INVOKABLE void saveToFile (const QString& filename); -// Q_INVOKABLE bool loadFromCurrent(); -// Q_INVOKABLE bool loadFromFile (const QString& filename); + Q_INVOKABLE bool upload(); + Q_INVOKABLE bool forceUpload(); + Q_INVOKABLE void removeFromVehicle(); // static Members -// static const char* wimaFileExtension; static const char* areaItemsName; static const char* missionItemsName; static const char* settingsGroup; @@ -294,78 +278,81 @@ public: static const char* snakeLineDistanceName; static const char* snakeMinTransectLengthName; - // Member Methodes - QJsonDocument saveToJson(FileType fileType); - - bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path); - signals: + // Controllers. void masterControllerChanged (void); void missionControllerChanged (void); + // Wima data. void visualItemsChanged (void); -// void currentFileChanged (); - void dataContainerChanged (); - void readyForSaveSendChanged (bool ready); + // Waypoints. void missionItemsChanged (void); void currentMissionItemsChanged (void); void waypointPathChanged (void); void currentWaypointPathChanged (void); - void uploadOverrideRequiredChanged (void); + // Smart RTL. + void smartRTLRequestConfirm (void); + void smartRTLPathConfirm (void); + // Upload. + void forceUploadConfirm (void); + // Waypoint statistics. void phaseDistanceChanged (void); void phaseDurationChanged (void); - void vehicleHasLowBatteryChanged (void); - void returnBatteryLowConfirmRequired (void); - void returnUserRequestConfirmRequired (void); + // Snake. void snakeConnectionStatusChanged (void); void snakeCalcInProgressChanged (void); void snakeTilesChanged (void); void snakeTileCenterPointsChanged (void); void nemoProgressChanged (void); -private: - enum SRTL_Reason {BatteryLow, UserRequest}; + private slots: - bool _fetchContainerData(); - bool _calcNextPhase(void); - //void _updateWaypointPath (void); - //void _updateCurrentPath (void); - //void _updateNextWaypoint (void); - void _recalcCurrentPhase (void); - //bool _setTakeoffLandPosition (void); - void _updateOverlap (void); - void _updateMaxWaypoints (void); - void _updateflightSpeed (void); - void _updateArrivalReturnSpeed (void); - void _updateAltitude (void); - void _checkBatteryLevel (void); - void _eventTimerHandler (void); - void _smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL - void _enableDisableLowBatteryHandling (QVariant enable); - void _initSmartRTL (); - void _executeSmartRTL (); + + // Waypoint navigation / helper. + bool _calcNextPhase (void); + void _recalcCurrentPhase (void); + bool _calcShortestPath (const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path); + // Slicing parameters + bool _setStartIndex (void); + void _updateOverlap (void); + void _updateMaxWaypoints (void); + // Waypoint settings. + void _updateflightSpeed (void); + void _updateArrivalReturnSpeed (void); + void _updateAltitude (void); + // Waypoint Statistics. + void _setPhaseDistance (double distance); + void _setPhaseDuration (double duration); + // SMART RTL + void _checkBatteryLevel (void); + QString _checkSmartRTLPreCondition (void); + void _initSmartRTL (); + void _smartRTLCleanUp (bool flying); + // Snake. void _setSnakeConnectionStatus (SnakeConnectionStatus status); void _setSnakeCalcInProgress (bool inProgress); - bool _verifyScenarioDefined (void); - bool _verifyScenarioDefinedWithErrorMessage (void); + bool _isScenarioDefined (void); + bool _isScenarioDefinedErrorMessage (void); void _snakeStoreWorkerResults (); void _startStopRosBridge (); void _initStartSnakeWorker (); + void _switchSnakeManager (QVariant variant); + // Periodic tasks. + void _eventTimerHandler (void); + // Waypoint manager handling. + void _switchWaypointManager(WaypointManager::ManagerBase &manager); 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); - void _loadCurrentMissionItemsFromBuffer(); - void _saveCurrentMissionItemsToBuffer(); - - void _progressFromJson(JsonDocUPtr pDoc, - QNemoProgress &progress); + // Snake. + void _progressFromJson (JsonDocUPtr pDoc, + QNemoProgress &progress); + // Controllers. PlanMasterController *_masterController; MissionController *_missionController; -// QString _currentFile; // file for saveing - WimaDataContainer *_container; // container for data exchange with WimaController + + // Wima Data. + QmlObjectListModel _areas; // contains all visible areas WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor WimaMeasurementAreaData _measurementArea; // measurement area @@ -373,22 +360,15 @@ private: WimaCorridorData _corridor; // corridor connecting opArea and serArea bool _localPlanDataValid; + // Waypoint Managers. + WaypointManager::AreaInterface _areaInterface; + WaypointManager::Settings _managerSettings; + WaypointManager::DefaultManager _defaultManager; + WaypointManager::DefaultManager _snakeManager; + WaypointManager::RTLManager _rtlManager; + WaypointManager::ManagerBase *_currentManager; - WaypointManager::AreaInterface _areaInterface; - WaypointManager::Settings _managerSettings; - WaypointManager::DefaultManager _defaultManager; - WaypointManager::DefaultManager _snakeManager; - WaypointManager::ManagerBase &_currentManager; - -// 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 -// QVector _waypoints; // path connecting the items in _missionItems -// QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems -// QGeoCoordinate _takeoffLandPostion; - - + // Settings Facts. QMap _metaDataMap; SettingsFact _enableWimaController; // enables or disables the wimaControler SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases @@ -401,48 +381,37 @@ private: SettingsFact _arrivalReturnSpeed; // arrival and return path speed SettingsFact _altitude; // mission altitude SettingsFact _enableSnake; // Enable Snake (see snake.h) - -// int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element -// // (which is not part of the return path) of _currentMissionItem -// int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element -// // (which is not part of the arrival path) of _currentMissionItem - bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. - // The user can override the upload lock with a slider, this will reset this variable to false. - double _measurementPathLength; // the lenght of the phase in meters -// double _arrivalPathLength; // the length of the arrival and return path in meters -// double _returnPathLength; // the length of the arrival and return path in meters -// double _phaseDistance; // the lenth in meters of the current phase -// double _phaseDuration; // the phase duration in seconds -// double _phaseDistanceBuffer; // buffer for storing _phaseDistance when doing smart RTL -// double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL - - QTimer _eventTimer; - QTimer _smartRTLAttemptTimer; - SRTL_Reason _srtlReason; - - bool _vehicleHasLowBattery; - bool _lowBatteryHandlingTriggered; - bool _executingSmartRTL; - - // Snake - SnakeConnectionStatus _snakeConnectionStatus; - bool _snakeCalcInProgress; - bool _snakeRecalcNecessary; - bool _scenarioDefinedBool; - SnakeWorker _snakeWorker; - Scenario _scenario; SettingsFact _snakeTileWidth; SettingsFact _snakeTileHeight; SettingsFact _snakeMinTileArea; SettingsFact _snakeLineDistance; SettingsFact _snakeMinTransectLength; - ::GeoPoint3D _snakeOrigin; - SnakeTiles _snakeTiles; // tiles - SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system - QVariantList _snakeTileCenterPoints; - QNemoProgress _nemoProgress; // measurement progress - ROSBridgePtr _pRosBridge; + // Smart RTL. + QTimer _smartRTLTimer; + bool _lowBatteryHandlingTriggered; + + // Waypoint statistics. + double _measurementPathLength; // the lenght of the phase in meters +// double _phaseDistance; // the lenth in meters of the current phase +// double _phaseDuration; // the phase duration in seconds + + // Snake + SnakeConnectionStatus _snakeConnectionStatus; + bool _snakeCalcInProgress; + bool _snakeRecalcNecessary; + bool _scenarioDefinedBool; + SnakeWorker _snakeWorker; + Scenario _scenario; + ::GeoPoint3D _snakeOrigin; + SnakeTiles _snakeTiles; // tiles + SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system + QVariantList _snakeTileCenterPoints; + QNemoProgress _nemoProgress; // measurement progress + ROSBridgePtr _pRosBridge; + + // Periodic tasks. + QTimer _eventTimer; }; diff --git a/src/Wima/WimaDataContainer.cc b/src/Wima/WimaDataContainer.cc deleted file mode 100644 index b5447007d29e43aceac9a6bd280135d73408d9bb..0000000000000000000000000000000000000000 --- a/src/Wima/WimaDataContainer.cc +++ /dev/null @@ -1,49 +0,0 @@ -#include "WimaDataContainer.h" - -WimaDataContainer::WimaDataContainer(QObject *parent) - : QObject (parent) - , _planData (this /* parent */) -{ - -} - -/*! - * \fn void WimaDataContainer::push(const WimaPlanData &planData) - * - * Updates the \c WimaPlanData members content with \a planData. - * Emits the planDataChanged() signal. - * - * \sa WimaPlanData - */ -void WimaDataContainer::push(const WimaPlanData &planData) -{ - _planData = planData; - - emit newDataAvailable(); -} - -/*! - * \fn const WimaPlanData &WimaDataContainer::pull() const - * - * Returns a constant referenc to the \c WimaPlanData member. - * - * \sa WimaPlanData - */ -const WimaPlanData &WimaDataContainer::pull() const -{ - return _planData; -} - -/*! - * \class WimaDataContainer - * \brief Data container designed for data exchange between \c WimaPlaner and \c WimaController. - * Data container designed for data exchange between \c WimaPlaner and \c WimaController. - * It is meant that only one instance of this class exists. Both \c WimaPlaner and \c WimaController - * have a reference to this instance and can modify its data. - * - * \sa WimaController, WimaPlaner - */ - - - - diff --git a/src/Wima/WimaDataContainer.h b/src/Wima/WimaDataContainer.h deleted file mode 100644 index 9de2eddef30fab426e0c1eb2345e759fc45205a7..0000000000000000000000000000000000000000 --- a/src/Wima/WimaDataContainer.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include - -#include "WimaPlanData.h" - -class WimaDataContainer : public QObject -{ - Q_OBJECT -public: - WimaDataContainer(QObject *parent = nullptr); - WimaDataContainer(WimaDataContainer &other, QObject *parent = nullptr) = delete; - WimaDataContainer(WimaDataContainer &other) = delete; - - Q_INVOKABLE WimaDataContainer* pointerToThis() {return this;} - -signals: - void newDataAvailable(void); - -public slots: - void push(const WimaPlanData &planData); - const WimaPlanData &pull() const; -private: - - - WimaPlanData _planData; -}; - diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 3cd9ce1459dde4af482aeb7a933de3f0e28ad0fe..a0cda2907906b85f39c53082213e7b1551e23807 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -11,7 +11,7 @@ const char* WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject (parent) , _currentAreaIndex (-1) - , _container (nullptr) + , _wimaBridge (nullptr) , _joinedArea (this) , _joinedAreaValid (false) , _measurementArea (this) @@ -39,7 +39,7 @@ WimaPlaner::WimaPlaner(QObject *parent) // for debugging and testing purpose, remove if not needed anymore connect(&_autoLoadTimer, &QTimer::timeout, this, &WimaPlaner::autoLoadMission); _autoLoadTimer.setSingleShot(true); - //_autoLoadTimer.start(300); + _autoLoadTimer.start(300); _calcArrivalAndReturnPathTimer.setInterval(100); _calcArrivalAndReturnPathTimer.setSingleShot(true); @@ -95,11 +95,11 @@ void WimaPlaner::setCurrentPolygonIndex(int index) } } -void WimaPlaner::setDataContainer(WimaDataContainer *container) +void WimaPlaner::setWimaBridge(WimaBridge *bridge) { - if (container != nullptr) { - _container = container; - emit dataContainerChanged(); + if (bridge != nullptr) { + _wimaBridge = bridge; + emit wimaBridgeChanged(); } } @@ -113,6 +113,11 @@ bool WimaPlaner::readyForSync() return _readyForSync; } +WimaPlaner *WimaPlaner::thisPointer() +{ + return this; +} + void WimaPlaner::removeArea(int index) { if(index >= 0 && index < _visualItems.count()){ @@ -636,20 +641,13 @@ bool WimaPlaner::recalcJoinedArea() return true; } -/*! - * \fn void WimaPlaner::pushToContainer() - * Pushes the \c WimaPlanData object generated by \c toPlanData() to the \c WimaDataContainer. - * Should be called only after \c updateMission() was successful. - * - * \sa WimaDataContainer, WimaPlanData - */ -void WimaPlaner::pushToContainer() +void WimaPlaner::pushToWimaController() { - if (_container != nullptr) { + if (_wimaBridge != nullptr) { if (!_readyForSync) return; WimaPlanData planData = toPlanData(); - _container->push(planData); + (void)_wimaBridge->setWimaPlanData(planData); setSyncronizedWithController(true); } else { qWarning("WimaPlaner::uploadToContainer(): no container assigned."); @@ -830,8 +828,8 @@ void WimaPlaner::setSyncronizedWithControllerFalse() void WimaPlaner::autoLoadMission() { - loadFromFile("/home/valentin/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release/345.wima"); - pushToContainer(); + loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/KlingenbachTest.wima"); + pushToWimaController(); } void WimaPlaner::startCalcArrivalAndReturnTimer() diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 7c0454458d0efe37e335f271b3e530b3bf2acd49..abf4ee7e63b3067851e312b6f85cfc70a8be4e16 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -15,7 +15,7 @@ #include "Geometry/WimaJoinedArea.h" #include "Geometry/WimaJoinedAreaData.h" #include "WimaPlanData.h" -#include "WimaDataContainer.h" +#include "WimaBridge.h" #include "PlanMasterController.h" #include "MissionController.h" @@ -60,7 +60,7 @@ public: Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) Q_PROPERTY(QGeoCoordinate joinedAreaCenter READ joinedAreaCenter CONSTANT) - Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged) + Q_PROPERTY(WimaBridge* wimaBridge READ wimaBridge WRITE setWimaBridge NOTIFY wimaBridgeChanged) Q_PROPERTY(bool syncronized READ syncronizedWithController NOTIFY syncronizedWithControllerChanged) Q_PROPERTY(bool readyForSync READ readyForSync NOTIFY readyForSyncChanged) @@ -74,20 +74,21 @@ public: QStringList saveNameFilters (void) const; QString fileExtension (void) const { return wimaFileExtension; } QGeoCoordinate joinedAreaCenter (void) const; - WimaDataContainer* dataContainer (void) { return _container;} + WimaBridge* wimaBridge (void) { return _wimaBridge;} // Property setters void setMasterController (PlanMasterController* masterController); void setMissionController (MissionController* missionController); /// Sets the integer index pointing to the current polygon. Current polygon is set interactive. void setCurrentPolygonIndex (int index); - void setDataContainer (WimaDataContainer* container); + void setWimaBridge (WimaBridge* bridge); // Property acessors bool syncronizedWithController (); bool readyForSync (); // Member Methodes + Q_INVOKABLE WimaPlaner *thisPointer(); Q_INVOKABLE bool addMeasurementArea(); /// Removes an area from _visualItems /// @param index Index of the area to be removed @@ -98,8 +99,8 @@ public: Q_INVOKABLE void removeAll(); /// Recalculates vehicle corridor, flight path, etc. Q_INVOKABLE bool updateMission(); - /// Pushes the generated mission data to the container, for exchange with wimaController - Q_INVOKABLE void pushToContainer(); + /// Pushes the generated mission data to the wimaController. + Q_INVOKABLE void pushToWimaController(); Q_INVOKABLE void saveToCurrent(); Q_INVOKABLE void saveToFile(const QString& filename); @@ -124,7 +125,7 @@ signals: void visualItemsChanged (void); void currentPolygonIndexChanged (int index); void currentFileChanged (); - void dataContainerChanged (); + void wimaBridgeChanged (); void syncronizedWithControllerChanged (void); void readyForSyncChanged (void); @@ -152,7 +153,7 @@ private: MissionController *_missionController; int _currentAreaIndex; QString _currentFile; // file for saveing - WimaDataContainer *_container; // container for data exchange with WimaController + WimaBridge *_wimaBridge; // container for data exchange with WimaController QmlObjectListModel _visualItems; // contains all visible areas WimaJoinedArea _joinedArea; // joined area fromed by _measurementArea, _serviceArea, _corridor bool _joinedAreaValid; diff --git a/src/WimaView/WimaToolBar.qml b/src/WimaView/WimaToolBar.qml index eea88cf8ac5418fdd3becf97cb00b56318fc616f..e2afb31c189ab3d0ddf29be12cfa7fc6547f9d5e 100644 --- a/src/WimaView/WimaToolBar.qml +++ b/src/WimaView/WimaToolBar.qml @@ -290,7 +290,7 @@ Rectangle { visible: wimaPlaner ? wimaPlaner.readyForSync : false onClicked: { if (wimaPlaner && wimaPlaner.readyForSync) { - wimaPlaner.pushToContainer() + wimaPlaner.pushToWimaController() } } diff --git a/src/WimaView/WimaView.qml b/src/WimaView/WimaView.qml index bd429e33660a2165073422919b502452b65864e7..a7b2a8bc32fa89593d4c33a8d7a15a3a18d7471f 100644 --- a/src/WimaView/WimaView.qml +++ b/src/WimaView/WimaView.qml @@ -207,7 +207,8 @@ QGCView { Component.onCompleted: { wimaPlaner.masterController = Qt.binding(function () { return masterController}) wimaPlaner.missionController = Qt.binding(function () { return masterController.missionController}) - wimaPlaner.dataContainer = Qt.binding(function () { return dataContainerPointer}) + wimaPlaner.wimaBridge = Qt.binding(function () { return wBridge.thisPointer()}) + wBridge.wimaPlaner = Qt.binding(function () { return wimaPlaner.thisPointer()}) } function addComplexItem(complexItemName) { var coordinate = editorMap.center diff --git a/src/ui/MainWindowInner.qml b/src/ui/MainWindowInner.qml index 4da272c8196b8891a084d819591507ccc26cbf8e..dc78d194f2adbc5cdb8975ad408a90fc2f9a17de 100644 --- a/src/ui/MainWindowInner.qml +++ b/src/ui/MainWindowInner.qml @@ -174,8 +174,8 @@ Item { } // Wima Data Container - WimaDataContainer{ - id: wimaDataContainer + WimaBridge{ + id: wimaBridge } MessageDialog { @@ -385,7 +385,7 @@ Item { visible: false property var planToolBar: planToolBar - property var dataContainer: wimaDataContainer + property var wBridge: wimaBridge } @@ -403,7 +403,7 @@ Item { visible: false property var toolbar: wimaToolBar - property var dataContainerPointer: wimaDataContainer.pointerToThis() + property var wBridge: wimaBridge } @@ -412,7 +412,7 @@ Item { anchors.fill: parent visible: true - property var dataContainerPointer: wimaDataContainer.pointerToThis() + property var wBridge: wimaBridge //------------------------------------------------------------------------- //-- Loader helper for any child, no matter how deep can display an element // on top of the video window.