Waypoint managers removed, code compiles

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 \

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<const WimaArea&>(area), outputString);
-}
-
 
 /*!
   \class WimaCorridor

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:

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<const WimaArea&>(area), outputString);
-}
-
-
 /*!
   \class WimaJoinedArea
   \brief Joined area (derived from \c WimaArea) composed by the \c WimaMeasurementArea, the \c WimaCorridor, and the \c WimaServiceArea.

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:

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<const WimaArea &>(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] {

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;

src/Wima/WaypointManager/AreaInterface.cpp

-#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

-#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

-#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

-#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

-#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

-#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.cpp

-#include "GenericWaypointManager.h"

src/Wima/WaypointManager/GenericWaypointManager.h

-#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

-#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

-#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

-#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

-#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

-#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";
-}