Skip to content

Q
qgroundcontrol
Commit 7427e2f7 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

Waypoint managers removed, code compiles

parent 3dc41359
Show whitespace changes
Inline Side-by-side
Showing with 27 additions and 1615 deletions
qgroundcontrol.pro
View file @ 7427e2f7
...@@ -486,16 +486,9 @@ HEADERS += \ ...@@ -486,16 +486,9 @@ HEADERS += \
src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \ src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/StateMachine.h \ src/Wima/Utils.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/WimaBridge.h \ src/Wima/WimaBridge.h \
src/Wima/WimaStateMachine.h \
src/Wima/call_once.h \ src/Wima/call_once.h \
src/api/QGCCorePlugin.h \ src/api/QGCCorePlugin.h \
src/api/QGCOptions.h \ src/api/QGCOptions.h \
...@@ -547,6 +540,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) { ...@@ -547,6 +540,7 @@ contains (DEFINES, QGC_ENABLE_PAIRING) {
SOURCES += \ SOURCES += \
src/Vehicle/VehicleEscStatusFactGroup.cc \ src/Vehicle/VehicleEscStatusFactGroup.cc \
src/Wima/WimaStateMachine.cpp \
src/api/QGCCorePlugin.cc \ src/api/QGCCorePlugin.cc \
src/api/QGCOptions.cc \ src/api/QGCOptions.cc \
src/api/QGCSettings.cc \ src/api/QGCSettings.cc \
...@@ -563,15 +557,7 @@ SOURCES += \ ...@@ -563,15 +557,7 @@ SOURCES += \
src/Wima/Snake/NemoInterface.cpp \ src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \ src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeTile.cpp \ src/Wima/Snake/SnakeTile.cpp \
src/Wima/StateMachine.cpp \ src/Wima/Utils.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/WimaBridge.cc \ src/Wima/WimaBridge.cc \
src/comm/ros_bridge/include/RosBridgeClient.cpp \ src/comm/ros_bridge/include/RosBridgeClient.cpp \
src/comm/ros_bridge/include/com_private.cpp \ src/comm/ros_bridge/include/com_private.cpp \
......
src/Wima/Geometry/WimaCorridor.cc
View file @ 7427e2f7
...@@ -51,18 +51,6 @@ void WimaCorridor::init() ...@@ -51,18 +51,6 @@ void WimaCorridor::init()
this->setObjectName(WimaCorridorName); 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<const WimaArea&>(area), outputString);
}
/*! /*!
\class WimaCorridor \class WimaCorridor
......
src/Wima/Geometry/WimaCorridor.h
View file @ 7427e2f7
...@@ -22,10 +22,6 @@ public: ...@@ -22,10 +22,6 @@ public:
// static Members // static Members
static const char *WimaCorridorName; static const char *WimaCorridorName;
// Friends
friend void print(const WimaCorridor &area, QString &outputString);
friend void print(const WimaCorridor &area);
signals: signals:
public slots: public slots:
......
src/Wima/Geometry/WimaJoinedArea.cc
View file @ 7427e2f7
...@@ -50,19 +50,6 @@ void WimaJoinedArea::init() ...@@ -50,19 +50,6 @@ void WimaJoinedArea::init()
this->setObjectName(WimaJoinedAreaName); 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<const WimaArea&>(area), outputString);
}
/*! /*!
\class WimaJoinedArea \class WimaJoinedArea
\brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea. \brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea.
......
src/Wima/Geometry/WimaJoinedArea.h
View file @ 7427e2f7
...@@ -25,10 +25,6 @@ public: ...@@ -25,10 +25,6 @@ public:
// static Members // static Members
static const char* WimaJoinedAreaName; static const char* WimaJoinedAreaName;
// Friends
friend void print(const WimaJoinedArea& area, QString& outputString);
friend void print(const WimaJoinedArea& area);
signals: signals:
public slots: public slots:
......
src/Wima/Geometry/WimaServiceArea.cc
View file @ 7427e2f7
...@@ -85,18 +85,6 @@ bool WimaServiceArea::loadFromJson(const QJsonObject &json, ...@@ -85,18 +85,6 @@ bool WimaServiceArea::loadFromJson(const QJsonObject &json,
return retVal; return retVal;
} }
void print(const WimaServiceArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
void print(const WimaServiceArea &area, QString &outputStr) {
print(static_cast<const WimaArea &>(area), outputStr);
outputStr.append(QString("Depot Position: %s\n")
.arg(area._depot.toString(QGeoCoordinate::Degrees)));
}
void WimaServiceArea::init() { void WimaServiceArea::init() {
this->setObjectName(wimaServiceAreaName); this->setObjectName(wimaServiceAreaName);
connect(this, &WimaArea::pathChanged, [this] { connect(this, &WimaArea::pathChanged, [this] {
......
src/Wima/Geometry/WimaServiceArea.h
View file @ 7427e2f7
...@@ -26,10 +26,6 @@ public: ...@@ -26,10 +26,6 @@ public:
void saveToJson(QJsonObject &json); void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString); 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 Members
static const char *wimaServiceAreaName; static const char *wimaServiceAreaName;
static const char *depotLatitudeName; static const char *depotLatitudeName;
......
src/Wima/WaypointManager/AreaInterface.cpp deleted 100644 → 0
View file @ 3dc41359
#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;
}
src/Wima/WaypointManager/AreaInterface.h deleted 100644 → 0
View file @ 3dc41359
#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
src/Wima/WaypointManager/DefaultManager.cpp deleted 100644 → 0
View file @ 3dc41359
#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<QGeoCoordinate> &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<QPointF> 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<QGeoCoordinate> 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<QGeoCoordinate> 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<MissionSettingsItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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;
}
src/Wima/WaypointManager/DefaultManager.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <QGeoCoordinate>
#include <QVector>
#include "GenericWaypointManager.h"
#include "QmlObjectListModel.h"
#include "Settings.h"
#include "AreaInterface.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate,
QVector,
QmlObjectListModel,
Settings>
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<QGeoCoordinate> &path);
AreaInterface *_areaInterface;
private:
bool _worker();
};
} // namespace WaypointManager
src/Wima/WaypointManager/EmptyManager.cpp deleted 100644 → 0
View file @ 3dc41359
#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
src/Wima/WaypointManager/EmptyManager.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <QGeoCoordinate>
#include <QVector>
#include "AreaInterface.h"
#include "GenericWaypointManager.h"
#include "QmlObjectListModel.h"
#include "Settings.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate, QVector, QmlObjectListModel,
Settings>
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
src/Wima/WaypointManager/GenericWaypointManager.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <iostream>
#include "Slicer.h"
namespace WaypointManager {
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
class GenericWaypointManager : public Slicer
{
public:
typedef ContainerType<WaypointType> 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 &currentWaypoints() const;
const MissionItemList &missionItems() const;
const MissionItemList &currentMissionItems() const;
const QVariantList &waypointsVariant() const;
const QVariantList &currentWaypointsVariant() 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 WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::GenericWaypointManager(SettingsType &settings)
: Slicer()
, _settings(&settings)
, _dirty(true)
{}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::push_back(const WaypointType &wp)
{
_dirty = true;
_waypoints.push_back(wp);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::push_front(
const WaypointType &wp)
{
_dirty = true;
_waypoints.push_front(wp);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clear();
_currentMissionItems.clear();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::insert(std::size_t i,
const WaypointType &wp)
{
_dirty = true;
_waypoints.insert(i, wp);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
std::size_t GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::size() const
{
return _waypoints.size();
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::remove(std::size_t i)
{
return _waypoints.remove(i);
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::waypointsVariant() const
{
return _waypointsVariant;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const QVariantList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentWaypointsVariant() const
{
return _currentWaypointsVariant;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const ContainerType<WaypointType> &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::waypoints() const
{
return _waypoints;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const ContainerType<WaypointType> &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentWaypoints() const
{
return _currentWaypoints;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const MissionItemList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::missionItems() const
{
return _missionItems;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
const MissionItemList &GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::currentMissionItems() const
{
return _currentMissionItems;
}
template<class WaypointType,
template<class, class...> class ContainerType,
class MissionItemList,
class SettingsType>
void GenericWaypointManager<WaypointType,
ContainerType,
MissionItemList,
SettingsType>::setWaypoints(const ContainerType<WaypointType> &waypoints)
{
_waypoints = waypoints;
}
} // namespace WaypointManager
src/Wima/WaypointManager/RTLManager.cpp deleted 100644 → 0
View file @ 3dc41359
#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<QGeoCoordinate> &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<QGeoCoordinate> &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<QPointF> 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<QGeoCoordinate> 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<MissionSettingsItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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<SimpleMissionItem *>(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;
}
src/Wima/WaypointManager/RTLManager.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include "GenericWaypointManager.h"
#include "Settings.h"
#include "AreaInterface.h"
namespace WaypointManager {
typedef GenericWaypointManager<QGeoCoordinate,
QVector,
QmlObjectListModel,
Settings>
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<QGeoCoordinate> &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<QGeoCoordinate> &path);
AreaInterface *_areaInterface;
private:
bool _worker();
};
} // namespace WaypointManager
src/Wima/WaypointManager/Settings.cpp deleted 100644 → 0
View file @ 3dc41359
#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;
}
src/Wima/WaypointManager/Settings.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <QGeoCoordinate>
#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
src/Wima/WaypointManager/Slicer.cpp deleted 100644 → 0
View file @ 3dc41359
#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";
}
src/Wima/WaypointManager/Slicer.h deleted 100644 → 0
View file @ 3dc41359
#pragma once
#include <assert.h>
#include <iostream>
#include "Utils.h"
//! @brief Helper class for slicing containers.
class Slicer
{
public:
Slicer();
protected:
// Slicing.
template <class Container1, class Container2>
void update (const Container1 &source, Container2 &slice);
template <class Container1, class Container2>
void next (const Container1 &source, Container2 &slice);
template <class Container1, class Container2>
void previous (const Container1 &source, Container2 &slice);
template <class Container1, class Container2>
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 <class Container1, class Container2>
void Slicer::update(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
WaypointManager::Utils::extract(source, slice, _idxStart, _idxEnd);
}
template <class Container1, class Container2>
void Slicer::next(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxNext);
update(source, slice);
}
template <class Container1, class Container2>
void Slicer::previous(const Container1 &source, Container2 &slice){
_updateIdx(source.size());
setStartIndex(_idxPrevious);
update(source, slice);
}
template <class Container1, class Container2>
void Slicer::reset(const Container1 &source, Container2 &slice){
setStartIndex(0);
update(source, slice);
}
src/Wima/WimaController.cc
View file @ 7427e2f7
...@@ -36,20 +36,12 @@ constexpr typename std::underlying_type<T>::type integral(T value) { ...@@ -36,20 +36,12 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
#define EVENT_TIMER_INTERVAL 50 // ms #define EVENT_TIMER_INTERVAL 50 // ms
const char *WimaController::areaItemsName = "AreaItems"; const char *WimaController::areaItemsName = "AreaItems";
const char *WimaController::missionItemsName = "MissionItems";
const char *WimaController::settingsGroup = "WimaController"; const char *WimaController::settingsGroup = "WimaController";
const char *WimaController::enableWimaControllerName = "EnableWimaController"; const char *WimaController::enableWimaControllerName = "EnableWimaController";
const char *WimaController::flightSpeedName = "FlightSpeed";
const char *WimaController::altitudeName = "Altitude";
WimaController::WimaController(QObject *parent) WimaController::WimaController(QObject *parent)
: QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(), : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(),
_corridor(), _planDataValid(false), _corridor(), _planDataValid(false),
_areaInterface(&_measurementArea, &_serviceArea, &_corridor,
&_joinedArea),
_WMSettings(), _emptyWM(_WMSettings, _areaInterface),
_rtlWM(_WMSettings, _areaInterface),
_currentWM(&_emptyWM), _WMList{&_emptyWM, &_rtlWM},
_metaDataMap(FactMetaData::createMapFromJsonFile( _metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)), QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
_enableWimaController(settingsGroup, _enableWimaController(settingsGroup,
...@@ -71,67 +63,18 @@ MissionController *WimaController::missionController() { ...@@ -71,67 +63,18 @@ MissionController *WimaController::missionController() {
QmlObjectListModel *WimaController::visualItems() { return &_areas; } QmlObjectListModel *WimaController::visualItems() { return &_areas; }
QmlObjectListModel *WimaController::missionItems() {
return const_cast<QmlObjectListModel *>(&_currentWM->currentMissionItems());
}
QVariantList WimaController::waypointPath() {
return const_cast<QVariantList &>(_currentWM->currentWaypointsVariant());
}
Fact *WimaController::enableWimaController() { return &_enableWimaController; } Fact *WimaController::enableWimaController() { return &_enableWimaController; }
Fact *WimaController::flightSpeed() { return &_flightSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
void WimaController::setMasterController(PlanMasterController *masterC) { void WimaController::setMasterController(PlanMasterController *masterC) {
_masterController = masterC; _masterController = masterC;
_WMSettings.setMasterController(masterC);
emit masterControllerChanged(); emit masterControllerChanged();
} }
void WimaController::setMissionController(MissionController *missionC) { void WimaController::setMissionController(MissionController *missionC) {
_missionController = missionC; _missionController = missionC;
_WMSettings.setMissionController(missionC);
emit missionControllerChanged(); 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<QGeoCoordinate> &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<QPointF> path2D;
bool retVal =
PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
toGeoList(path2D, /*origin*/ start, path);
return retVal;
}
void WimaController::planDataChangedHandler() { void WimaController::planDataChangedHandler() {
// reset visual items // reset visual items
...@@ -143,8 +86,6 @@ void WimaController::planDataChangedHandler() { ...@@ -143,8 +86,6 @@ void WimaController::planDataChangedHandler() {
_planDataValid = false; _planDataValid = false;
emit visualItemsChanged(); emit visualItemsChanged();
emit missionItemsChanged();
emit waypointPathChanged();
// Extract areas. // Extract areas.
auto planData = WimaBridge::instance()->planData(); auto planData = WimaBridge::instance()->planData();
...@@ -159,10 +100,6 @@ void WimaController::planDataChangedHandler() { ...@@ -159,10 +100,6 @@ void WimaController::planDataChangedHandler() {
_serviceArea = planData.serviceArea(); _serviceArea = planData.serviceArea();
_areas.append(&_serviceArea); _areas.append(&_serviceArea);
_WMSettings.setHomePosition(
QGeoCoordinate(_serviceArea.depot().latitude(),
_serviceArea.depot().longitude(), 0));
// Joined Area. // Joined Area.
if (planData.joinedArea().coordinateList().size() >= 3) { if (planData.joinedArea().coordinateList().size() >= 3) {
_joinedArea = planData.joinedArea(); _joinedArea = planData.joinedArea();
...@@ -192,33 +129,3 @@ void WimaController::planDataChangedHandler() { ...@@ -192,33 +129,3 @@ void WimaController::planDataChangedHandler() {
void WimaController::progressChangedHandler() { void WimaController::progressChangedHandler() {
_measurementArea.setProgress(WimaBridge::instance()->progress()); _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();
}
src/Wima/WimaController.h
View file @ 7427e2f7
#pragma once #pragma once
#include <QObject> #include <QObject>
#include <QScopedPointer>
#include "QGCMapPolygon.h" #include "QGCMapPolygon.h"
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include "Geometry/WimaCorridorData.h" #include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceAreaData.h" #include "Geometry/WimaServiceAreaData.h"
#include "SettingsFact.h" #include "SettingsFact.h"
#include "Geometry/GeoPoint3D.h" class MissionController;
#include "RoutingThread.h" class PlanMasterController;
#include "Snake/NemoInterface.h"
#include "WaypointManager/EmptyManager.h"
#include "WaypointManager/RTLManager.h"
#include "utilities.h"
#include <map>
typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
class WimaController : public QObject { class WimaController : public QObject {
Q_OBJECT Q_OBJECT
...@@ -38,51 +28,27 @@ public: ...@@ -38,51 +28,27 @@ public:
// Wima Data. // Wima Data.
Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY
visualItemsChanged) visualItemsChanged)
Q_PROPERTY(QmlObjectListModel *missionItems READ missionItems NOTIFY
missionItemsChanged)
Q_PROPERTY(
QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged)
Q_PROPERTY(Fact *enableWimaController READ enableWimaController CONSTANT) Q_PROPERTY(Fact *enableWimaController READ enableWimaController CONSTANT)
// Waypoint navigaton.
Q_PROPERTY(Fact *flightSpeed READ flightSpeed CONSTANT)
Q_PROPERTY(Fact *altitude READ altitude CONSTANT)
// Controllers. // Controllers.
PlanMasterController *masterController(void); PlanMasterController *masterController(void);
MissionController *missionController(void); MissionController *missionController(void);
// Wima Data // Wima Data
QmlObjectListModel *visualItems(void); QmlObjectListModel *visualItems(void);
QGCMapPolygon joinedArea(void) const; QGCMapPolygon joinedArea(void) const;
// Waypoints.
QmlObjectListModel *missionItems(void);
QVariantList waypointPath(void);
// Settings facts. // Settings facts.
Fact *enableWimaController(void); Fact *enableWimaController(void);
Fact *flightSpeed(void);
Fact *altitude(void);
bool uploadOverrideRequired(void) const;
bool vehicleHasLowBattery(void) const;
// Property setters // Property setters
void setMasterController(PlanMasterController *masterController); void setMasterController(PlanMasterController *masterController);
void setMissionController(MissionController *missionController); 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 Members
static const char *areaItemsName; static const char *areaItemsName;
static const char *missionItemsName;
static const char *settingsGroup; static const char *settingsGroup;
static const char *enableWimaControllerName; static const char *enableWimaControllerName;
static const char *flightSpeedName;
static const char *altitudeName;
signals: signals:
// Controllers. // Controllers.
...@@ -90,24 +56,10 @@ signals: ...@@ -90,24 +56,10 @@ signals:
void missionControllerChanged(void); void missionControllerChanged(void);
// Wima data. // Wima data.
void visualItemsChanged(void); void visualItemsChanged(void);
// Waypoints.
void missionItemsChanged(void);
void waypointPathChanged(void);
// Upload.
void forceUploadConfirm(void);
private slots: private slots:
void planDataChangedHandler(); void planDataChangedHandler();
void progressChangedHandler(); void progressChangedHandler();
bool _calcShortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path);
void _updateflightSpeed(void);
void _updateAltitude(void);
// Snake.
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
// Periodic tasks.
void _eventTimerHandler(void);
private: private:
// Controllers. // Controllers.
...@@ -123,24 +75,7 @@ private: ...@@ -123,24 +75,7 @@ private:
WimaCorridorData _corridor; // corridor connecting opArea and serArea WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _planDataValid; 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<WaypointManager::ManagerBase *>;
ManagerList _WMList;
// Settings Facts. // Settings Facts.
QMap<QString, FactMetaData *> _metaDataMap; QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _enableWimaController; // enables or disables the wimaControler 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;
}; };
src/Wima/WimaPlaner.cc
View file @ 7427e2f7
...@@ -17,7 +17,7 @@ ...@@ -17,7 +17,7 @@
#include "Geometry/WimaAreaData.h" #include "Geometry/WimaAreaData.h"
#include "WimaBridge.h" #include "WimaBridge.h"
#include "StateMachine.h" #include "WimaStateMachine.h"
using namespace wima_planer_detail; using namespace wima_planer_detail;
#include <functional> #include <functional>
...@@ -39,7 +39,7 @@ const char *WimaPlaner::missionItemsName = "MissionItems"; ...@@ -39,7 +39,7 @@ const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent) WimaPlaner::WimaPlaner(QObject *parent)
: QObject(parent), _masterController(nullptr), _missionController(nullptr), : QObject(parent), _masterController(nullptr), _missionController(nullptr),
_currentAreaIndex(-1), _joinedArea(this), _survey(nullptr), _currentAreaIndex(-1), _joinedArea(this), _survey(nullptr),
_nemoInterface(this), _stateMachine(new StateMachine), _nemoInterface(this), _stateMachine(new WimaStateMachine),
_areasMonitored(false), _missionControllerMonitored(false), _areasMonitored(false), _missionControllerMonitored(false),
_progressLocked(false), _synchronized(false) { _progressLocked(false), _synchronized(false) {
...@@ -63,11 +63,11 @@ WimaPlaner::WimaPlaner(QObject *parent) ...@@ -63,11 +63,11 @@ WimaPlaner::WimaPlaner(QObject *parent)
&WimaPlaner::nemoInterfaceProgressChangedHandler); &WimaPlaner::nemoInterfaceProgressChangedHandler);
// StateMachine // StateMachine
connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this, connect(this->_stateMachine.get(), &WimaStateMachine::upToDateChanged, this,
&WimaPlaner::needsUpdateChanged); &WimaPlaner::needsUpdateChanged);
connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this,
&WimaPlaner::readyForSynchronizationChanged); &WimaPlaner::readyForSynchronizationChanged);
connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, connect(this->_stateMachine.get(), &WimaStateMachine::surveyReadyChanged, this,
&WimaPlaner::surveyReadyChanged); &WimaPlaner::surveyReadyChanged);
} }
......
src/Wima/WimaPlaner.h
View file @ 7427e2f7
...@@ -23,7 +23,7 @@ class MissionController; ...@@ -23,7 +23,7 @@ class MissionController;
class PlanMasterController; class PlanMasterController;
namespace wima_planer_detail { namespace wima_planer_detail {
class StateMachine; class WimaStateMachine;
} }
class WimaPlaner : public QObject { class WimaPlaner : public QObject {
...@@ -187,7 +187,7 @@ private: ...@@ -187,7 +187,7 @@ private:
NemoInterface _nemoInterface; NemoInterface _nemoInterface;
// State // State
QScopedPointer<wima_planer_detail::StateMachine> _stateMachine; QScopedPointer<wima_planer_detail::WimaStateMachine> _stateMachine;
bool _areasMonitored; bool _areasMonitored;
bool _missionControllerMonitored; bool _missionControllerMonitored;
bool _progressLocked; bool _progressLocked;
......
src/Wima/StateMachine.cpp → src/Wima/WimaStateMachine.cpp
View file @ 7427e2f7
#include "StateMachine.h" #include "WimaStateMachine.h"
#include "QGCLoggingCategory.h" #include "QGCLoggingCategory.h"
...@@ -13,12 +13,12 @@ constexpr typename std::underlying_type<T>::type integral(T value) { ...@@ -13,12 +13,12 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value); return static_cast<typename std::underlying_type<T>::type>(value);
} }
StateMachine::StateMachine(QObject *parent) WimaStateMachine::WimaStateMachine(QObject *parent)
: QObject(parent), _state(STATE::NEEDS_INIT) {} : 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; qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e;
switch (this->_state) { switch (this->_state) {
case STATE::NEEDS_INIT: case STATE::NEEDS_INIT:
...@@ -240,11 +240,11 @@ void StateMachine::updateState(EVENT e) { ...@@ -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) { if (this->_state != s) {
auto oldState = this->_state; auto oldState = this->_state;
this->_state = s; this->_state = s;
...@@ -260,7 +260,7 @@ void StateMachine::setState(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. // Using a switch to enable compiler checking of used states.
bool value = false; bool value = false;
switch (s) { switch (s) {
...@@ -278,7 +278,7 @@ bool StateMachine::surveyReady(STATE s) { ...@@ -278,7 +278,7 @@ bool StateMachine::surveyReady(STATE s) {
return value; return value;
} }
bool StateMachine::upToDate(STATE s) { bool WimaStateMachine::upToDate(STATE s) {
// Using a switch to enable compiler checking of used states. // Using a switch to enable compiler checking of used states.
bool value = false; bool value = false;
switch (s) { switch (s) {
......
src/Wima/StateMachine.h → src/Wima/WimaStateMachine.h
View file @ 7427e2f7
...@@ -38,10 +38,10 @@ enum class EVENT { ...@@ -38,10 +38,10 @@ enum class EVENT {
QDebug &operator<<(QDebug &ds, EVENT s); QDebug &operator<<(QDebug &ds, EVENT s);
class StateMachine : public QObject { class WimaStateMachine : public QObject {
Q_OBJECT Q_OBJECT
public: public:
explicit StateMachine(QObject *parent = nullptr); explicit WimaStateMachine(QObject *parent = nullptr);
STATE state(); STATE state();
void updateState(EVENT e); void updateState(EVENT e);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment