From 7427e2f7f1a4c7085be245bf2861557037411416 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 11 Nov 2020 10:54:16 +0100 Subject: [PATCH] Waypoint managers removed, code compiles --- qgroundcontrol.pro | 22 +- src/Wima/Geometry/WimaCorridor.cc | 12 - src/Wima/Geometry/WimaCorridor.h | 4 - src/Wima/Geometry/WimaJoinedArea.cc | 13 - src/Wima/Geometry/WimaJoinedArea.h | 4 - src/Wima/Geometry/WimaServiceArea.cc | 12 - src/Wima/Geometry/WimaServiceArea.h | 4 - src/Wima/WaypointManager/AreaInterface.cpp | 82 ------ src/Wima/WaypointManager/AreaInterface.h | 42 --- src/Wima/WaypointManager/DefaultManager.cpp | 277 ------------------ src/Wima/WaypointManager/DefaultManager.h | 56 ---- src/Wima/WaypointManager/EmptyManager.cpp | 18 -- src/Wima/WaypointManager/EmptyManager.h | 31 -- .../GenericWaypointManager.cpp | 1 - .../WaypointManager/GenericWaypointManager.h | 239 --------------- src/Wima/WaypointManager/RTLManager.cpp | 219 -------------- src/Wima/WaypointManager/RTLManager.h | 99 ------- src/Wima/WaypointManager/Settings.cpp | 100 ------- src/Wima/WaypointManager/Settings.h | 47 --- src/Wima/WaypointManager/Slicer.cpp | 67 ----- src/Wima/WaypointManager/Slicer.h | 89 ------ src/Wima/WimaController.cc | 93 ------ src/Wima/WimaController.h | 75 +---- src/Wima/WimaPlaner.cc | 10 +- src/Wima/WimaPlaner.h | 4 +- ...{StateMachine.cpp => WimaStateMachine.cpp} | 18 +- .../{StateMachine.h => WimaStateMachine.h} | 4 +- 27 files changed, 27 insertions(+), 1615 deletions(-) delete mode 100644 src/Wima/WaypointManager/AreaInterface.cpp delete mode 100644 src/Wima/WaypointManager/AreaInterface.h delete mode 100644 src/Wima/WaypointManager/DefaultManager.cpp delete mode 100644 src/Wima/WaypointManager/DefaultManager.h delete mode 100644 src/Wima/WaypointManager/EmptyManager.cpp delete mode 100644 src/Wima/WaypointManager/EmptyManager.h delete mode 100644 src/Wima/WaypointManager/GenericWaypointManager.cpp delete mode 100644 src/Wima/WaypointManager/GenericWaypointManager.h delete mode 100644 src/Wima/WaypointManager/RTLManager.cpp delete mode 100644 src/Wima/WaypointManager/RTLManager.h delete mode 100644 src/Wima/WaypointManager/Settings.cpp delete mode 100644 src/Wima/WaypointManager/Settings.h delete mode 100644 src/Wima/WaypointManager/Slicer.cpp delete mode 100644 src/Wima/WaypointManager/Slicer.h rename src/Wima/{StateMachine.cpp => WimaStateMachine.cpp} (95%) rename src/Wima/{StateMachine.h => WimaStateMachine.h} (91%) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 61c87295f..694cd0a6d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -486,16 +486,9 @@ HEADERS += \ src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTilesLocal.h \ - src/Wima/StateMachine.h \ - src/Wima/WaypointManager/AreaInterface.h \ - src/Wima/WaypointManager/DefaultManager.h \ - src/Wima/WaypointManager/EmptyManager.h \ - src/Wima/WaypointManager/GenericWaypointManager.h \ - src/Wima/WaypointManager/RTLManager.h \ - src/Wima/WaypointManager/Settings.h \ - src/Wima/WaypointManager/Slicer.h \ - src/Wima/WaypointManager/Utils.h \ + src/Wima/Utils.h \ src/Wima/WimaBridge.h \ + src/Wima/WimaStateMachine.h \ src/Wima/call_once.h \ src/api/QGCCorePlugin.h \ src/api/QGCOptions.h \ @@ -547,6 +540,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { SOURCES += \ src/Vehicle/VehicleEscStatusFactGroup.cc \ + src/Wima/WimaStateMachine.cpp \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ src/api/QGCSettings.cc \ @@ -563,15 +557,7 @@ SOURCES += \ src/Wima/Snake/NemoInterface.cpp \ src/Wima/Snake/QNemoProgress.cc \ src/Wima/Snake/SnakeTile.cpp \ - src/Wima/StateMachine.cpp \ - src/Wima/WaypointManager/AreaInterface.cpp \ - src/Wima/WaypointManager/DefaultManager.cpp \ - src/Wima/WaypointManager/EmptyManager.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/Utils.cpp \ src/Wima/WimaBridge.cc \ src/comm/ros_bridge/include/RosBridgeClient.cpp \ src/comm/ros_bridge/include/com_private.cpp \ diff --git a/src/Wima/Geometry/WimaCorridor.cc b/src/Wima/Geometry/WimaCorridor.cc index aebf438b9..52c0659d2 100644 --- a/src/Wima/Geometry/WimaCorridor.cc +++ b/src/Wima/Geometry/WimaCorridor.cc @@ -51,18 +51,6 @@ void WimaCorridor::init() this->setObjectName(WimaCorridorName); } -void print(const WimaCorridor &area) -{ - QString message; - print(area, message); - qWarning() << message; -} - -void print(const WimaCorridor &area, QString &outputString) -{ - print(static_cast(area), outputString); -} - /*! \class WimaCorridor diff --git a/src/Wima/Geometry/WimaCorridor.h b/src/Wima/Geometry/WimaCorridor.h index aecaa66ba..cef6494ed 100644 --- a/src/Wima/Geometry/WimaCorridor.h +++ b/src/Wima/Geometry/WimaCorridor.h @@ -22,10 +22,6 @@ public: // static Members static const char *WimaCorridorName; - // Friends - friend void print(const WimaCorridor &area, QString &outputString); - friend void print(const WimaCorridor &area); - signals: public slots: diff --git a/src/Wima/Geometry/WimaJoinedArea.cc b/src/Wima/Geometry/WimaJoinedArea.cc index f59658e88..83bbd58ce 100644 --- a/src/Wima/Geometry/WimaJoinedArea.cc +++ b/src/Wima/Geometry/WimaJoinedArea.cc @@ -50,19 +50,6 @@ void WimaJoinedArea::init() this->setObjectName(WimaJoinedAreaName); } -void print(const WimaJoinedArea &area) -{ - QString message; - print(area, message); - qWarning() << message; -} - -void print(const WimaJoinedArea &area, QString &outputString) -{ - print(static_cast(area), outputString); -} - - /*! \class WimaJoinedArea \brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea. diff --git a/src/Wima/Geometry/WimaJoinedArea.h b/src/Wima/Geometry/WimaJoinedArea.h index 33897bae2..2ecdce3d0 100644 --- a/src/Wima/Geometry/WimaJoinedArea.h +++ b/src/Wima/Geometry/WimaJoinedArea.h @@ -25,10 +25,6 @@ public: // static Members static const char* WimaJoinedAreaName; - // Friends - friend void print(const WimaJoinedArea& area, QString& outputString); - friend void print(const WimaJoinedArea& area); - signals: public slots: diff --git a/src/Wima/Geometry/WimaServiceArea.cc b/src/Wima/Geometry/WimaServiceArea.cc index c8fb2071d..84b6c6afd 100644 --- a/src/Wima/Geometry/WimaServiceArea.cc +++ b/src/Wima/Geometry/WimaServiceArea.cc @@ -85,18 +85,6 @@ bool WimaServiceArea::loadFromJson(const QJsonObject &json, return retVal; } -void print(const WimaServiceArea &area) { - QString message; - print(area, message); - qWarning() << message; -} - -void print(const WimaServiceArea &area, QString &outputStr) { - print(static_cast(area), outputStr); - outputStr.append(QString("Depot Position: %s\n") - .arg(area._depot.toString(QGeoCoordinate::Degrees))); -} - void WimaServiceArea::init() { this->setObjectName(wimaServiceAreaName); connect(this, &WimaArea::pathChanged, [this] { diff --git a/src/Wima/Geometry/WimaServiceArea.h b/src/Wima/Geometry/WimaServiceArea.h index 6bbb615bf..08ff94c34 100644 --- a/src/Wima/Geometry/WimaServiceArea.h +++ b/src/Wima/Geometry/WimaServiceArea.h @@ -26,10 +26,6 @@ public: void saveToJson(QJsonObject &json); bool loadFromJson(const QJsonObject &json, QString &errorString); - // Friends - friend void print(const WimaServiceArea &area, QString &outputStr); - friend void print(const WimaServiceArea &area); - // static Members static const char *wimaServiceAreaName; static const char *depotLatitudeName; diff --git a/src/Wima/WaypointManager/AreaInterface.cpp b/src/Wima/WaypointManager/AreaInterface.cpp deleted file mode 100644 index d2ba5b105..000000000 --- a/src/Wima/WaypointManager/AreaInterface.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "AreaInterface.h" - -WaypointManager::AreaInterface::AreaInterface() - : _mArea(nullptr) - , _sArea(nullptr) - , _cArea(nullptr) - , _jArea(nullptr) -{ - -} - -WaypointManager::AreaInterface::AreaInterface(WimaMeasurementAreaData *mArea, - WimaServiceAreaData *sArea, - WimaCorridorData *corr, - WimaJoinedAreaData *jArea) - : _mArea(mArea) - , _sArea(sArea) - , _cArea(corr) - , _jArea(jArea) -{ - -} - -void WaypointManager::AreaInterface::setMeasurementArea(WimaMeasurementAreaData *area) -{ - _mArea = area; -} - -void WaypointManager::AreaInterface::setServiceArea(WimaServiceAreaData *area) -{ - _sArea = area; -} - -void WaypointManager::AreaInterface::setCorridor(WimaCorridorData *area) -{ - _cArea = area; -} - -void WaypointManager::AreaInterface::setJoinedArea(WimaJoinedAreaData *area) -{ - _jArea = area; -} - -const WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea() const -{ - return _mArea; -} - -const WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea() const -{ - return _sArea; -} - -const WimaCorridorData *WaypointManager::AreaInterface::corridor() const -{ - return _cArea; -} - -const WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea() const -{ - return _jArea; -} - -WimaMeasurementAreaData *WaypointManager::AreaInterface::measurementArea() -{ - return _mArea; -} - -WimaServiceAreaData *WaypointManager::AreaInterface::serviceArea() -{ - return _sArea; -} - -WimaCorridorData *WaypointManager::AreaInterface::corridor() -{ - return _cArea; -} - -WimaJoinedAreaData *WaypointManager::AreaInterface::joinedArea() -{ - return _jArea; -} diff --git a/src/Wima/WaypointManager/AreaInterface.h b/src/Wima/WaypointManager/AreaInterface.h deleted file mode 100644 index 431c23eda..000000000 --- a/src/Wima/WaypointManager/AreaInterface.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include "Wima/Geometry/WimaMeasurementAreaData.h" -#include "Wima/Geometry/WimaServiceAreaData.h" -#include "Wima/Geometry/WimaCorridorData.h" -#include "Wima/Geometry/WimaJoinedAreaData.h" - -namespace WaypointManager { - -class AreaInterface -{ -public: - AreaInterface(); - AreaInterface(WimaMeasurementAreaData *mArea, - WimaServiceAreaData *sArea, - WimaCorridorData *corr, - WimaJoinedAreaData *jArea); - - void setMeasurementArea (WimaMeasurementAreaData *area); - void setServiceArea (WimaServiceAreaData *area); - void setCorridor (WimaCorridorData *area); - void setJoinedArea (WimaJoinedAreaData *area); - - virtual const WimaMeasurementAreaData *measurementArea() const; - virtual const WimaServiceAreaData *serviceArea() const; - virtual const WimaCorridorData *corridor() const; - virtual const WimaJoinedAreaData *joinedArea() const; - - virtual WimaMeasurementAreaData *measurementArea(); - virtual WimaServiceAreaData *serviceArea(); - virtual WimaCorridorData *corridor(); - virtual WimaJoinedAreaData *joinedArea(); - -private: - WimaMeasurementAreaData *_mArea; - WimaServiceAreaData *_sArea; - WimaCorridorData *_cArea; - WimaJoinedAreaData *_jArea; -}; - - -} // namespace WaypointManager diff --git a/src/Wima/WaypointManager/DefaultManager.cpp b/src/Wima/WaypointManager/DefaultManager.cpp deleted file mode 100644 index eca4e4f1e..000000000 --- a/src/Wima/WaypointManager/DefaultManager.cpp +++ /dev/null @@ -1,277 +0,0 @@ -#include "DefaultManager.h" -#include "Wima/Geometry/GeoUtilities.h" -#include "Wima/Geometry/PolygonCalculus.h" - -#include "MissionSettingsItem.h" -#include "SimpleMissionItem.h" - -WaypointManager::DefaultManager::DefaultManager(Settings &settings, - AreaInterface &interface) - : ManagerBase(settings), _areaInterface(&interface) {} - -void WaypointManager::DefaultManager::clear() { - _dirty = true; - _waypoints.clear(); - _currentWaypoints.clear(); - _missionItems.clearAndDeleteContents(); - _currentMissionItems.clearAndDeleteContents(); - _waypointsVariant.clear(); - _currentWaypointsVariant.clear(); -} - -bool WaypointManager::DefaultManager::update() { - // extract waypoints - _currentWaypoints.clear(); - Slicer::update(_waypoints, _currentWaypoints); - return _worker(); -} - -bool WaypointManager::DefaultManager::next() { - // extract waypoints - _currentWaypoints.clear(); - Slicer::next(_waypoints, _currentWaypoints); - return _worker(); -} - -bool WaypointManager::DefaultManager::previous() { - // extract waypoints - _currentWaypoints.clear(); - Slicer::previous(_waypoints, _currentWaypoints); - return _worker(); -} - -bool WaypointManager::DefaultManager::reset() { - // extract waypoints - _currentWaypoints.clear(); - Slicer::reset(_waypoints, _currentWaypoints); - return _worker(); -} - -bool WaypointManager::DefaultManager::_insertMissionItem( - const QGeoCoordinate &c, size_t index, QmlObjectListModel &list, - bool doUpdate) { - using namespace WaypointManager::Utils; - - if (!insertMissionItem(c, index /*insertion index*/, list, - _settings->masterController(), _settings->isFlyView(), - &list /*parent*/, doUpdate /*do update*/)) { - qWarning( - "WaypointManager::DefaultManager::next(): insertMissionItem failed."); - Q_ASSERT(false); - return false; - } - return true; -} - -bool WaypointManager::DefaultManager::_insertMissionItem( - const QGeoCoordinate &c, size_t index, bool doUpdate) { - return _insertMissionItem(c, index, _currentMissionItems, doUpdate); -} - -bool WaypointManager::DefaultManager::_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::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()) { - return false; - } - - if (_dirty) { - _missionItems.clearAndDeleteContents(); - _waypointsVariant.clear(); - // No initialization of _missionItems, don't need MissionSettingsItem here. - for (size_t i = 0; i < size_t(_waypoints.size()); ++i) { - auto &c = _waypoints.at(i); - _insertMissionItem(c, _missionItems.count(), _missionItems, - false /*update list*/); - _waypointsVariant.push_back(QVariant::fromValue(c)); - } - updateHirarchy(_missionItems); - updateSequenceNumbers( - _missionItems, 1); // Start with 1, since MissionSettingsItem missing. - _dirty = false; - } - - _currentMissionItems.clearAndDeleteContents(); - initialize(_currentMissionItems, _settings->masterController(), - _settings->isFlyView()); - - // Calculate path from home to first waypoint. - QVector arrivalPath; - if (!_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), - arrivalPath)) { - qWarning("WaypointManager::DefaultManager::next(): Not able to calc path " - "from home position to first waypoint."); - qWarning() << "from: " << _settings->homePosition(); - qWarning() << "to: " << _currentWaypoints.first(); - return false; - } - if (!arrivalPath.empty()) - arrivalPath.removeFirst(); - if (!arrivalPath.empty()) - arrivalPath.removeLast(); - - // calculate path from last waypoint to home - QVector returnPath; - if (!_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), - returnPath)) { - qWarning("WaypointManager::DefaultManager::next(): Not able to calc path " - "from home position to last waypoint."); - qWarning() << "from: " << _currentWaypoints.last(); - qWarning() << "to: " << _settings->homePosition(); - return false; - } - if (!returnPath.empty()) - returnPath.removeFirst(); - if (!returnPath.empty()) - returnPath.removeLast(); - - // Create mission items. - // Set home position of MissionSettingsItem. - MissionSettingsItem *settingsItem = - _currentMissionItems.value(0); - if (settingsItem == nullptr) { - Q_ASSERT(false); - qWarning("WaypointManager::DefaultManager::next(): nullptr."); - return false; - } - settingsItem->setCoordinate(_settings->homePosition()); - - // First mission item is takeoff command. - _insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, - false /*do update*/); - SimpleMissionItem *takeoffItem = - _currentMissionItems.value(1); - if (takeoffItem == nullptr) { - qWarning("WaypointManager::DefaultManager::next(): nullptr."); - Q_ASSERT(takeoffItem != nullptr); - return false; - } - makeTakeOffCmd(takeoffItem, _settings->masterController()); - - // Create change speed item. - _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, - false /*do update*/); - SimpleMissionItem *speedItem = - _currentMissionItems.value(2); - if (speedItem == nullptr) { - qWarning("WaypointManager::DefaultManager::next(): nullptr."); - Q_ASSERT(speedItem != nullptr); - return false; - } - makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed()); - - // insert arrival path - for (auto coordinate : arrivalPath) { - _insertMissionItem(coordinate, - _currentMissionItems.count() /*insertion index*/, - false /*do update*/); - } - - // Create change speed item (after arrival path). - int index = _currentMissionItems.count(); - _insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, - false /*do update*/); - speedItem = _currentMissionItems.value(index); - if (speedItem == nullptr) { - Q_ASSERT(false); - qWarning("WaypointManager::DefaultManager::next(): nullptr."); - return false; - } - makeSpeedCmd(speedItem, _settings->flightSpeed()); - - // Insert slice. - for (auto coordinate : _currentWaypoints) { - _insertMissionItem(coordinate, - _currentMissionItems.count() /*insertion index*/, - false /*do update*/); - } - - // Create change speed item, after slice. - index = _currentMissionItems.count(); - _insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, - false /*do update*/); - speedItem = _currentMissionItems.value(index); - if (speedItem == nullptr) { - Q_ASSERT(false); - qWarning("WaypointManager::DefaultManager::next(): 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. - 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::DefaultManager::next(): nullptr."); - return false; - } - makeLandCmd(landItem, _settings->masterController()); - - // 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)); - - return true; -} diff --git a/src/Wima/WaypointManager/DefaultManager.h b/src/Wima/WaypointManager/DefaultManager.h deleted file mode 100644 index 21b168177..000000000 --- a/src/Wima/WaypointManager/DefaultManager.h +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include -#include - -#include "GenericWaypointManager.h" -#include "QmlObjectListModel.h" -#include "Settings.h" -#include "AreaInterface.h" - -namespace WaypointManager { - -typedef GenericWaypointManager - ManagerBase; -//! -//! \brief The DefaultManager class is used to manage SimpleMissionItems. -//! -//! @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 DefaultManager : public ManagerBase -{ -public: - DefaultManager() = delete; - DefaultManager(Settings &settings, - AreaInterface &interface); - - void clear() override; - - - virtual bool update() override; - virtual bool next() override; - virtual bool previous() override; - virtual bool reset() override; - -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/EmptyManager.cpp b/src/Wima/WaypointManager/EmptyManager.cpp deleted file mode 100644 index 60a84ce53..000000000 --- a/src/Wima/WaypointManager/EmptyManager.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "EmptyManager.h" - -namespace WaypointManager { - -WaypointManager::EmptyManager::EmptyManager(Settings &settings, AreaInterface &) - : ManagerBase(settings) {} - -void WaypointManager::EmptyManager::clear() {} - -bool WaypointManager::EmptyManager::update() { return true; } - -bool WaypointManager::EmptyManager::next() { return true; } - -bool WaypointManager::EmptyManager::previous() { return true; } - -bool WaypointManager::EmptyManager::reset() { return true; } - -} // namespace WaypointManager diff --git a/src/Wima/WaypointManager/EmptyManager.h b/src/Wima/WaypointManager/EmptyManager.h deleted file mode 100644 index 8ddfc1c3d..000000000 --- a/src/Wima/WaypointManager/EmptyManager.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#include - -#include "AreaInterface.h" -#include "GenericWaypointManager.h" -#include "QmlObjectListModel.h" -#include "Settings.h" - -namespace WaypointManager { - -typedef GenericWaypointManager - ManagerBase; - -//! -//! \brief The EmptyManager is a place holder and does noting. -class EmptyManager : public ManagerBase { -public: - EmptyManager() = delete; - EmptyManager(Settings &settings, AreaInterface &); - - void clear() override; - virtual bool update() override; - virtual bool next() override; - virtual bool previous() override; - virtual bool reset() override; -}; - -} // namespace WaypointManager diff --git a/src/Wima/WaypointManager/GenericWaypointManager.cpp b/src/Wima/WaypointManager/GenericWaypointManager.cpp deleted file mode 100644 index 489faec25..000000000 --- a/src/Wima/WaypointManager/GenericWaypointManager.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "GenericWaypointManager.h" diff --git a/src/Wima/WaypointManager/GenericWaypointManager.h b/src/Wima/WaypointManager/GenericWaypointManager.h deleted file mode 100644 index e50fc8407..000000000 --- a/src/Wima/WaypointManager/GenericWaypointManager.h +++ /dev/null @@ -1,239 +0,0 @@ -#pragma once - -#include - -#include "Slicer.h" - -namespace WaypointManager { - -template class ContainerType, - class MissionItemList, - class SettingsType> -class GenericWaypointManager : public Slicer -{ -public: - typedef ContainerType WaypointList; - - GenericWaypointManager() = delete; - GenericWaypointManager(SettingsType &settings); - - // Waypoint editing. - 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; - const WaypointList ¤tWaypoints() const; - const MissionItemList &missionItems() const; - const MissionItemList ¤tMissionItems() const; - - const QVariantList &waypointsVariant() const; - const QVariantList ¤tWaypointsVariant() const; - - virtual bool update() = 0; - virtual bool next() = 0; - virtual bool previous() = 0; - virtual bool reset() = 0; - -protected: - WaypointList _waypoints; - WaypointList _currentWaypoints; - MissionItemList _currentMissionItems; - MissionItemList _missionItems; - SettingsType *_settings; - bool _dirty; - QVariantList _waypointsVariant; - QVariantList _currentWaypointsVariant; -}; - - -template class ContainerType, - class MissionItemList, - class SettingsType> -GenericWaypointManager::GenericWaypointManager(SettingsType &settings) - : Slicer() - , _settings(&settings) - , _dirty(true) -{} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::push_back(const WaypointType &wp) -{ - _dirty = true; - _waypoints.push_back(wp); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::push_front( - const WaypointType &wp) -{ - _dirty = true; - _waypoints.push_front(wp); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::clear() -{ - _dirty = true; - _waypoints.clear(); - _currentWaypoints.clear(); - _missionItems.clear(); - _currentMissionItems.clear(); - _waypointsVariant.clear(); - _currentWaypointsVariant.clear(); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::insert(std::size_t i, - const WaypointType &wp) -{ - _dirty = true; - _waypoints.insert(i, wp); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -std::size_t GenericWaypointManager::size() const -{ - return _waypoints.size(); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::remove(std::size_t i) -{ - return _waypoints.remove(i); -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const QVariantList &GenericWaypointManager::waypointsVariant() const -{ - return _waypointsVariant; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const QVariantList &GenericWaypointManager::currentWaypointsVariant() const -{ - return _currentWaypointsVariant; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const ContainerType &GenericWaypointManager::waypoints() const -{ - return _waypoints; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const ContainerType &GenericWaypointManager::currentWaypoints() const -{ - return _currentWaypoints; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const MissionItemList &GenericWaypointManager::missionItems() const -{ - return _missionItems; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -const MissionItemList &GenericWaypointManager::currentMissionItems() const -{ - return _currentMissionItems; -} - -template class ContainerType, - class MissionItemList, - class SettingsType> -void GenericWaypointManager::setWaypoints(const ContainerType &waypoints) -{ - _waypoints = waypoints; -} - - - -} // namespace WaypointManager - diff --git a/src/Wima/WaypointManager/RTLManager.cpp b/src/Wima/WaypointManager/RTLManager.cpp deleted file mode 100644 index 88b6fc253..000000000 --- a/src/Wima/WaypointManager/RTLManager.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include "RTLManager.h" -#include "Wima/Geometry/GeoUtilities.h" -#include "Wima/Geometry/PolygonCalculus.h" - -#include "MissionSettingsItem.h" -#include "SimpleMissionItem.h" - -WaypointManager::RTLManager::RTLManager(Settings &settings, - AreaInterface &interface) - : ManagerBase(settings), _areaInterface(&interface) {} - -void WaypointManager::RTLManager::setWaypoints( - const QVector &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; } - -bool WaypointManager::RTLManager::checkPrecondition(QString &errorString) { - // Be aware of the error message order! - Vehicle *managerVehicle = _settings->masterController()->managerVehicle(); - - if (managerVehicle->isOfflineEditingVehicle()) { - errorString.append("No vehicle connected. Smart RTL not available."); - return false; - } - - if (!managerVehicle->flying()) { - errorString.append("Vehicle is not flying. Smart RTL not available."); - return false; - } - - if (!_areaInterface->joinedArea()->containsCoordinate( - managerVehicle->coordinate())) { - errorString.append( - "Vehicle not inside save area. Smart RTL not available."); - return false; - } - - return true; -} - -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->masterController(), _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; - } - - _currentMissionItems.clearAndDeleteContents(); - initialize(_currentMissionItems, _settings->masterController(), - _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(vehicleCoordinate); - - // Create change speed item. - int index = _currentMissionItems.count(); - _insertMissionItem(vehicleCoordinate, index /*insertion index*/, - false /*do update*/); - SimpleMissionItem *speedItem = - _currentMissionItems.value(index); - 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) { - index = _currentMissionItems.count(); - _insertMissionItem(coordinate, index /*insertion index*/, - false /*do update*/); - } - // Set land command for last mission item. - 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()); - - // 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 deleted file mode 100644 index bacc49068..000000000 --- a/src/Wima/WaypointManager/RTLManager.h +++ /dev/null @@ -1,99 +0,0 @@ -#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; - - bool checkPrecondition(QString &errorString); - -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/Settings.cpp b/src/Wima/WaypointManager/Settings.cpp deleted file mode 100644 index 0a7934be1..000000000 --- a/src/Wima/WaypointManager/Settings.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "Settings.h" - -WaypointManager::Settings::Settings() - : _missionController(nullptr) - , _isFlyView(true) - , _arrivalReturnSpeed(5) - , _flightSpeed(2) - , _altitude(5) -{ - -} - -bool WaypointManager::Settings::valid() const -{ - return _missionController != nullptr - && _homePosition.isValid(); -} - -void WaypointManager::Settings::setHomePosition(const QGeoCoordinate &c) -{ - _homePosition = c; -} - -void WaypointManager::Settings::setMissionController(MissionController *controller) -{ - _missionController = controller; -} - -void WaypointManager::Settings::setMasterController(PlanMasterController *controller) -{ - _masterController = controller; -} - -void WaypointManager::Settings::setIsFlyView(bool isFlyView) -{ - _isFlyView = isFlyView; -} - -void WaypointManager::Settings::setArrivalReturnSpeed(double speed) -{ - if (speed > 0) - _arrivalReturnSpeed = speed; -} - -void WaypointManager::Settings::setFlightSpeed(double speed) -{ - if ( speed > 0) - _flightSpeed = speed; -} - -void WaypointManager::Settings::setAltitude(double altitude) -{ - if ( altitude > 0) - _altitude = altitude; -} - -const QGeoCoordinate &WaypointManager::Settings::homePosition() const -{ - return _homePosition; -} - -QGeoCoordinate &WaypointManager::Settings::homePosition() -{ - return _homePosition; -} - -MissionController *WaypointManager::Settings::missionController() const -{ - return _missionController; -} - -PlanMasterController *WaypointManager::Settings::masterController() const -{ - return _masterController; -} - -Vehicle *WaypointManager::Settings::vehicle() const -{ - return _masterController->managerVehicle(); -} - -bool WaypointManager::Settings::isFlyView() const -{ - return _isFlyView; -} - -double WaypointManager::Settings::arrivalReturnSpeed() const -{ - return _arrivalReturnSpeed; -} - -double WaypointManager::Settings::flightSpeed() const -{ - return _flightSpeed; -} - -double WaypointManager::Settings::altitude() const -{ - return _altitude; -} diff --git a/src/Wima/WaypointManager/Settings.h b/src/Wima/WaypointManager/Settings.h deleted file mode 100644 index a9ce345e3..000000000 --- a/src/Wima/WaypointManager/Settings.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include - -#include "MissionController.h" -#include "PlanMasterController.h" -#include "Vehicle.h" - -namespace WaypointManager { - -class Settings -{ -public: - Settings(); - - bool valid() const; - - void setHomePosition(const QGeoCoordinate &c); - void setMissionController(MissionController *controller); - void setMasterController(PlanMasterController *controller); - void setIsFlyView(bool isFlyView); - void setArrivalReturnSpeed(double speed); - void setFlightSpeed(double speed); - void setAltitude(double altitude); - - const QGeoCoordinate &homePosition() const; - QGeoCoordinate &homePosition(); - MissionController *missionController() const; - PlanMasterController *masterController() const; - Vehicle *vehicle() const; - bool isFlyView() const; - double arrivalReturnSpeed() const; - double flightSpeed() const; - double altitude() const; - -private: - QGeoCoordinate _homePosition; - MissionController *_missionController; - PlanMasterController *_masterController; - bool _isFlyView; - double _arrivalReturnSpeed; - double _flightSpeed; - double _altitude; - -}; - -} // namespace WaypointManager diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp deleted file mode 100644 index 6b13a0e01..000000000 --- a/src/Wima/WaypointManager/Slicer.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "Slicer.h" - -Slicer::Slicer() - : _idxStart(0) - , _idxEnd(0) - , _idxNext(0) - , _idxPrevious(0) - , _overlap(0) - , _N(0) -{} - -void Slicer::setOverlap(uint32_t overlap) -{ - _overlap = overlap; -} - -void Slicer::setN(uint32_t N) -{ - _N = N > 0 ? N : 1; -} - -void Slicer::setStartIndex(int idxStart) -{ - _idxStart = idxStart; -} - -uint32_t Slicer::overlap() -{ - return _overlap; -} - -uint32_t Slicer::N() -{ - return _N; -} - -int Slicer::startIndex() -{ - return _idxStart; -} - -void Slicer::_updateIdx(long size) -{ - _overlap = _overlap < _N ? _overlap : _N-1; - - _idxStart = _idxStart < size ? _idxStart : size-1; - _idxStart = _idxStart < 0 ? 0 : _idxStart; - - _idxEnd = _idxStart + _N - 1; - _idxEnd = _idxEnd < size ? _idxEnd : size-1; - - _idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap; - _idxNext = _idxNext < 0 ? 0 : _idxNext; - _idxNext = _idxNext < size ? _idxNext : 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 deleted file mode 100644 index 354db75cd..000000000 --- a/src/Wima/WaypointManager/Slicer.h +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include -#include - -#include "Utils.h" - -//! @brief Helper class for slicing containers. -class Slicer -{ -public: - Slicer(); - - -protected: - // Slicing. - template - void update (const Container1 &source, Container2 &slice); - template - void next (const Container1 &source, Container2 &slice); - template - void previous (const Container1 &source, Container2 &slice); - template - void reset (const Container1 &source, Container2 &slice); - -public: - // Slicing parameters. - //! @brief Sets the overlap. - //! - //! @param overlap The number of overlapping vertices - //! between on and the next slice. - void setOverlap (uint32_t overlap); - //! @brief Sets the number of vertices per slice. - //! - //! @param N The number of vertices per slice. - void setN (std::uint32_t N); - //! @brief Sets the start index. - //! - //! @param idxStart The start index. - void setStartIndex (int idxStart); - - //! @return Returns the overlap. - uint32_t overlap (); - //! @return Returns the number of vertices per slice N. - uint32_t N (); - //! @return Returns the start index. - int startIndex (); - - -private: - void _updateIdx(long size); - - long _idxStart; - long _idxEnd; - long _idxNext; - long _idxPrevious; - long _overlap; - long _N; - -}; - -template -void Slicer::update(const Container1 &source, Container2 &slice){ - - _updateIdx(source.size()); - WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd); -} - -template -void Slicer::next(const Container1 &source, Container2 &slice){ - _updateIdx(source.size()); - setStartIndex(_idxNext); - update(source, slice); -} - - -template -void Slicer::previous(const Container1 &source, Container2 &slice){ - _updateIdx(source.size()); - setStartIndex(_idxPrevious); - update(source, slice); -} - - -template -void Slicer::reset(const Container1 &source, Container2 &slice){ - setStartIndex(0); - update(source, slice); -} diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 422e786b4..3dc0a34e3 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -36,20 +36,12 @@ constexpr typename std::underlying_type::type integral(T value) { #define EVENT_TIMER_INTERVAL 50 // ms const char *WimaController::areaItemsName = "AreaItems"; -const char *WimaController::missionItemsName = "MissionItems"; const char *WimaController::settingsGroup = "WimaController"; const char *WimaController::enableWimaControllerName = "EnableWimaController"; -const char *WimaController::flightSpeedName = "FlightSpeed"; -const char *WimaController::altitudeName = "Altitude"; WimaController::WimaController(QObject *parent) : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(), _corridor(), _planDataValid(false), - _areaInterface(&_measurementArea, &_serviceArea, &_corridor, - &_joinedArea), - _WMSettings(), _emptyWM(_WMSettings, _areaInterface), - _rtlWM(_WMSettings, _areaInterface), - _currentWM(&_emptyWM), _WMList{&_emptyWM, &_rtlWM}, _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)), _enableWimaController(settingsGroup, @@ -71,67 +63,18 @@ MissionController *WimaController::missionController() { QmlObjectListModel *WimaController::visualItems() { return &_areas; } -QmlObjectListModel *WimaController::missionItems() { - return const_cast(&_currentWM->currentMissionItems()); -} - -QVariantList WimaController::waypointPath() { - return const_cast(_currentWM->currentWaypointsVariant()); -} - Fact *WimaController::enableWimaController() { return &_enableWimaController; } -Fact *WimaController::flightSpeed() { return &_flightSpeed; } - -Fact *WimaController::altitude() { return &_altitude; } - void WimaController::setMasterController(PlanMasterController *masterC) { _masterController = masterC; - _WMSettings.setMasterController(masterC); emit masterControllerChanged(); } void WimaController::setMissionController(MissionController *missionC) { _missionController = missionC; - _WMSettings.setMissionController(missionC); emit missionControllerChanged(); } -bool WimaController::upload() { - auto &items = _currentWM->currentMissionItems(); - if (_masterController && _masterController->managerVehicle() && - items.count() > 0) { - if (!_joinedArea.containsCoordinate( - _masterController->managerVehicle()->coordinate())) { - emit forceUploadConfirm(); - return false; - } else { - return forceUpload(); - } - } else { - return false; - } -} - -bool WimaController::_calcShortestPath(const QGeoCoordinate &start, - const QGeoCoordinate &destination, - QVector &path) { - using namespace GeoUtilities; - using namespace PolygonCalculus; - QPolygonF polygon2D; - toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); - QPointF start2D(0, 0); - QPointF end2D; - toCartesian(destination, start, end2D); - QVector path2D; - - bool retVal = - PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); - toGeoList(path2D, /*origin*/ start, path); - - return retVal; -} - void WimaController::planDataChangedHandler() { // reset visual items @@ -143,8 +86,6 @@ void WimaController::planDataChangedHandler() { _planDataValid = false; emit visualItemsChanged(); - emit missionItemsChanged(); - emit waypointPathChanged(); // Extract areas. auto planData = WimaBridge::instance()->planData(); @@ -159,10 +100,6 @@ void WimaController::planDataChangedHandler() { _serviceArea = planData.serviceArea(); _areas.append(&_serviceArea); - _WMSettings.setHomePosition( - QGeoCoordinate(_serviceArea.depot().latitude(), - _serviceArea.depot().longitude(), 0)); - // Joined Area. if (planData.joinedArea().coordinateList().size() >= 3) { _joinedArea = planData.joinedArea(); @@ -192,33 +129,3 @@ void WimaController::planDataChangedHandler() { void WimaController::progressChangedHandler() { _measurementArea.setProgress(WimaBridge::instance()->progress()); } - -WimaController *WimaController::thisPointer() { return this; } - -void WimaController::_updateflightSpeed() { - bool value; - _WMSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value)); - Q_ASSERT(value); - (void)value; - - if (!_currentWM->update()) { - Q_ASSERT(false); - } - - emit missionItemsChanged(); - emit waypointPathChanged(); -} - -void WimaController::_updateAltitude() { - bool value; - _WMSettings.setAltitude(_altitude.rawValue().toDouble(&value)); - Q_ASSERT(value); - (void)value; - - if (!_currentWM->update()) { - Q_ASSERT(false); - } - - emit missionItemsChanged(); - emit waypointPathChanged(); -} diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 2b359e75b..27b938bf0 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -1,29 +1,19 @@ #pragma once #include -#include #include "QGCMapPolygon.h" #include "QmlObjectListModel.h" #include "Geometry/WimaCorridorData.h" +#include "Geometry/WimaJoinedAreaData.h" #include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaServiceAreaData.h" #include "SettingsFact.h" -#include "Geometry/GeoPoint3D.h" -#include "RoutingThread.h" -#include "Snake/NemoInterface.h" - -#include "WaypointManager/EmptyManager.h" -#include "WaypointManager/RTLManager.h" - -#include "utilities.h" - -#include - -typedef std::unique_ptr JsonDocUPtr; +class MissionController; +class PlanMasterController; class WimaController : public QObject { Q_OBJECT @@ -38,51 +28,27 @@ public: // Wima Data. Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY visualItemsChanged) - Q_PROPERTY(QmlObjectListModel *missionItems READ missionItems NOTIFY - missionItemsChanged) - Q_PROPERTY( - QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged) Q_PROPERTY(Fact *enableWimaController READ enableWimaController CONSTANT) - // Waypoint navigaton. - Q_PROPERTY(Fact *flightSpeed READ flightSpeed CONSTANT) - Q_PROPERTY(Fact *altitude READ altitude CONSTANT) // Controllers. PlanMasterController *masterController(void); MissionController *missionController(void); + // Wima Data QmlObjectListModel *visualItems(void); QGCMapPolygon joinedArea(void) const; - // Waypoints. - QmlObjectListModel *missionItems(void); - QVariantList waypointPath(void); + // Settings facts. Fact *enableWimaController(void); - Fact *flightSpeed(void); - Fact *altitude(void); - - bool uploadOverrideRequired(void) const; - bool vehicleHasLowBattery(void) const; // Property setters void setMasterController(PlanMasterController *masterController); void setMissionController(MissionController *missionController); - // Member Methodes - Q_INVOKABLE WimaController *thisPointer(); - // Other. - Q_INVOKABLE void removeVehicleTrajectoryHistory(); - Q_INVOKABLE bool upload(); - Q_INVOKABLE bool forceUpload(); - Q_INVOKABLE void removeFromVehicle(); - // static Members static const char *areaItemsName; - static const char *missionItemsName; static const char *settingsGroup; static const char *enableWimaControllerName; - static const char *flightSpeedName; - static const char *altitudeName; signals: // Controllers. @@ -90,24 +56,10 @@ signals: void missionControllerChanged(void); // Wima data. void visualItemsChanged(void); - // Waypoints. - void missionItemsChanged(void); - void waypointPathChanged(void); - // Upload. - void forceUploadConfirm(void); private slots: void planDataChangedHandler(); void progressChangedHandler(); - bool _calcShortestPath(const QGeoCoordinate &start, - const QGeoCoordinate &destination, - QVector &path); - void _updateflightSpeed(void); - void _updateAltitude(void); - // Snake. - void _switchWaypointManager(WaypointManager::ManagerBase &manager); - // Periodic tasks. - void _eventTimerHandler(void); private: // Controllers. @@ -123,24 +75,7 @@ private: WimaCorridorData _corridor; // corridor connecting opArea and serArea bool _planDataValid; - // Waypoint Managers. - WaypointManager::AreaInterface _areaInterface; - WaypointManager::Settings _WMSettings; // Waypoint Manager Settings - WaypointManager::EmptyManager _emptyWM; - WaypointManager::RTLManager _rtlWM; - WaypointManager::ManagerBase *_currentWM; - using ManagerList = QList; - ManagerList _WMList; - // Settings Facts. QMap _metaDataMap; SettingsFact _enableWimaController; // enables or disables the wimaControler - // determines the number of overlapping waypoints between two consecutive - // mission phases - SettingsFact _flightSpeed; // mission flight speed - SettingsFact _altitude; // mission altitude - - // Periodic tasks. - QTimer _eventTimer; - EventTicker _batteryLevelTicker; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 6de56f90a..737d71681 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -17,7 +17,7 @@ #include "Geometry/WimaAreaData.h" #include "WimaBridge.h" -#include "StateMachine.h" +#include "WimaStateMachine.h" using namespace wima_planer_detail; #include @@ -39,7 +39,7 @@ const char *WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject(parent), _masterController(nullptr), _missionController(nullptr), _currentAreaIndex(-1), _joinedArea(this), _survey(nullptr), - _nemoInterface(this), _stateMachine(new StateMachine), + _nemoInterface(this), _stateMachine(new WimaStateMachine), _areasMonitored(false), _missionControllerMonitored(false), _progressLocked(false), _synchronized(false) { @@ -63,11 +63,11 @@ WimaPlaner::WimaPlaner(QObject *parent) &WimaPlaner::nemoInterfaceProgressChangedHandler); // StateMachine - connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this, + connect(this->_stateMachine.get(), &WimaStateMachine::upToDateChanged, this, &WimaPlaner::needsUpdateChanged); - connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, + connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this, &WimaPlaner::readyForSynchronizationChanged); - connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, + connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this, &WimaPlaner::surveyReadyChanged); } diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 7e4eab68d..758ac333d 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -23,7 +23,7 @@ class MissionController; class PlanMasterController; namespace wima_planer_detail { -class StateMachine; +class WimaStateMachine; } class WimaPlaner : public QObject { @@ -187,7 +187,7 @@ private: NemoInterface _nemoInterface; // State - QScopedPointer _stateMachine; + QScopedPointer _stateMachine; bool _areasMonitored; bool _missionControllerMonitored; bool _progressLocked; diff --git a/src/Wima/StateMachine.cpp b/src/Wima/WimaStateMachine.cpp similarity index 95% rename from src/Wima/StateMachine.cpp rename to src/Wima/WimaStateMachine.cpp index 5b4f85760..3c630f4ea 100644 --- a/src/Wima/StateMachine.cpp +++ b/src/Wima/WimaStateMachine.cpp @@ -1,4 +1,4 @@ -#include "StateMachine.h" +#include "WimaStateMachine.h" #include "QGCLoggingCategory.h" @@ -13,12 +13,12 @@ constexpr typename std::underlying_type::type integral(T value) { return static_cast::type>(value); } -StateMachine::StateMachine(QObject *parent) +WimaStateMachine::WimaStateMachine(QObject *parent) : QObject(parent), _state(STATE::NEEDS_INIT) {} -STATE StateMachine::state() { return this->_state; } +STATE WimaStateMachine::state() { return this->_state; } -void StateMachine::updateState(EVENT e) { +void WimaStateMachine::updateState(EVENT e) { qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e; switch (this->_state) { case STATE::NEEDS_INIT: @@ -240,11 +240,11 @@ void StateMachine::updateState(EVENT e) { } } -bool StateMachine::upToDate() { return upToDate(this->_state); } +bool WimaStateMachine::upToDate() { return upToDate(this->_state); } -bool StateMachine::surveyReady() { return surveyReady(this->_state); } +bool WimaStateMachine::surveyReady() { return surveyReady(this->_state); } -void StateMachine::setState(STATE s) { +void WimaStateMachine::setState(STATE s) { if (this->_state != s) { auto oldState = this->_state; this->_state = s; @@ -260,7 +260,7 @@ void StateMachine::setState(STATE s) { } } -bool StateMachine::surveyReady(STATE s) { +bool WimaStateMachine::surveyReady(STATE s) { // Using a switch to enable compiler checking of used states. bool value = false; switch (s) { @@ -278,7 +278,7 @@ bool StateMachine::surveyReady(STATE s) { return value; } -bool StateMachine::upToDate(STATE s) { +bool WimaStateMachine::upToDate(STATE s) { // Using a switch to enable compiler checking of used states. bool value = false; switch (s) { diff --git a/src/Wima/StateMachine.h b/src/Wima/WimaStateMachine.h similarity index 91% rename from src/Wima/StateMachine.h rename to src/Wima/WimaStateMachine.h index f98642ebf..884d0666c 100644 --- a/src/Wima/StateMachine.h +++ b/src/Wima/WimaStateMachine.h @@ -38,10 +38,10 @@ enum class EVENT { QDebug &operator<<(QDebug &ds, EVENT s); -class StateMachine : public QObject { +class WimaStateMachine : public QObject { Q_OBJECT public: - explicit StateMachine(QObject *parent = nullptr); + explicit WimaStateMachine(QObject *parent = nullptr); STATE state(); void updateState(EVENT e); -- 2.22.0