wima controller mod, appimage updated

qgroundcontrol.pro

@@ -448,7 +448,6 @@ HEADERS += \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QNemoProgress.h \
-    src/Wima/Snake/SnakeThread.h \
     src/Wima/Snake/SnakeTile.h \
     src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
@@ -513,7 +512,6 @@ SOURCES += \
     src/Wima/Geometry/GeoPoint3D.cpp \
     src/Wima/Snake/NemoInterface.cpp \
     src/Wima/Snake/QNemoProgress.cc \
-    src/Wima/Snake/SnakeThread.cc \
     src/Wima/Snake/SnakeTile.cpp \
     src/Wima/WaypointManager/AreaInterface.cpp \
     src/Wima/WaypointManager/DefaultManager.cpp \

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -241,43 +241,66 @@ FlightMap {
         color:          "green"
     }
 
-    // Add Snake tile center points to the map
-    MapItemView {
-        property bool _enable:    wimaController.enableWimaController.value
-                               && wimaController.enableSnake.value
-        model: _enable ? wimaController.snakeTileCenterPoints : 0
-
-        delegate: ProgressIndicator{
-            coordinate:         modelData
-            currentValue:   getProgress()
-            width:          10
-            height:         10
-            z:              1
-
-            function getProgress() {
-                var progress = wimaController.nemoProgress[index]
-                if (progress < 0)
-                    progress = 0
-                if (progress > 100)
-                    progress = 100
-                return progress
-            }
-        }
-    }
+//    // Add Snake tile center points to the map
+//    MapItemView {
+//        id:progressView
+//        property bool _enable:    wimaController.enableWimaController.value
+//                               && wimaController.enableSnake.value
+//        property bool valid: wimaController.snakeTileCenterPoints.length
+//                             === wimaController.nemoProgress.length
+//        model: _enable ? wimaController.snakeTileCenterPoints : 0
+
+//        delegate: ProgressIndicator{
+//            coordinate:         modelData
+//            currentValue:   getProgress()
+//            z:              1
+
+//            function getProgress() {
+//                var progress = 0
+//                if (progressView.valid){
+//                    progress = wimaController.nemoProgress[index]
+//                }
+//                if (progress < 0)
+//                    progress = 0
+//                if (progress > 100)
+//                    progress = 100
+//                return progress
+//            }
+//        }
+//    }
 
     // Add Snake tiles to the map
     MapItemView {
+            id: tileView
         property bool _enable:    wimaController.enableWimaController.value
                                && wimaController.enableSnake.value
+        property bool valid: wimaController.snakeTileCenterPoints.length
+                             === wimaController.nemoProgress.length
         model: _enable ? wimaController.snakeTiles : 0
 
         delegate: MapPolygon{
             path:           object.path;
             border.color:   "black"
             border.width:   1
-            color:          "transparent"
-            opacity:        1
+            color:          getColor()
+            opacity:        0.6
             z:              2
+
+            function getColor() {
+                var progress = 0
+                if (tileView.valid){
+                    progress = wimaController.nemoProgress[index]
+                }
+                if (progress < 25)
+                    return "transparent"
+                if (progress < 50)
+                    return "orange"
+                if (progress < 75)
+                    return "yellow"
+                if (progress < 100)
+                    return "greenyellow"
+                return "limegreen"
+            }
         }
     }
 

src/QmlControls/QmlObjectListModel.cc

@@ -121,9 +121,9 @@ const QObject *QmlObjectListModel::operator[](int index) const {
   return _objectList[index];
 }
 
-bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
+bool QmlObjectListModel::operator==(const QmlObjectListModel &other) const {
   if (this->count() == other.count()) {
-    for (std::size_t i = 0; i < this->count(); ++i) {
+    for (int i = 0; i < this->count(); ++i) {
       if (this->get(i) != other.get(i)) {
         return false;
       }
@@ -134,7 +134,7 @@ bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
   }
 }
 
-bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
+bool QmlObjectListModel::operator!=(const QmlObjectListModel &other) const {
   return !this->operator==(other);
 }
 

src/QmlControls/QmlObjectListModel.h

@@ -51,8 +51,8 @@ public:
   }
   QList<QObject *> *objectList() { return &_objectList; }
 
-  bool operator==(const QmlObjectListModel &other);
-  bool operator!=(const QmlObjectListModel &other);
+  bool operator==(const QmlObjectListModel &other) const;
+  bool operator!=(const QmlObjectListModel &other) const;
 
   /// Calls deleteLater on all items and this itself.
   void deleteListAndContents();

src/Wima/CircularSurvey.cc

@@ -359,7 +359,12 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
     // Store raw transects.
     const auto &transectsENU = this->_workerOutput->transects;
     const auto &ori = this->_referencePoint;
-    for (auto &t : transectsENU) {
+    std::size_t startIdx = 0;
+    if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
+      startIdx = 1;
+    }
+    for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
+      const auto &t = transectsENU[i];
       QList<QGeoCoordinate> trGeo;
       for (auto &v : t) {
         QGeoCoordinate c;

src/Wima/Geometry/WimaMeasurementArea.cc

@@ -28,6 +28,15 @@ TileData &TileData::operator=(const TileData &other) {
   return *this;
 }
 
+bool TileData::operator==(const TileData &other) const {
+  return this->tiles == other.tiles &&
+         this->tileCenterPoints == other.tileCenterPoints;
+}
+
+bool TileData::operator!=(const TileData &other) const {
+  return !this->operator==(other);
+}
+
 void TileData::clear() {
   this->tiles.clearAndDeleteContents();
   this->tileCenterPoints.clear();

src/Wima/Geometry/WimaMeasurementArea.h

@@ -17,6 +17,8 @@ public:
   ~TileData();
 
   TileData &operator=(const TileData &other);
+  bool operator==(const TileData &other) const;
+  bool operator!=(const TileData &other) const;
 
   void clear();
   std::size_t size() const;

src/Wima/Geometry/WimaMeasurementAreaData.cc

@@ -61,10 +61,10 @@ QVariantList &WimaMeasurementAreaData::tileCenterPoints() {
 }
 
 const TileData &WimaMeasurementAreaData::tileData() const {
-  return this->tileData();
+  return this->_tileData;
 }
 
-TileData &WimaMeasurementAreaData::tileData() { return this->tileData(); }
+TileData &WimaMeasurementAreaData::tileData() { return this->_tileData; }
 
 const QVector<int> &WimaMeasurementAreaData::progress() const {
   return this->_progress;

src/Wima/RoutingThread.cpp

@@ -21,7 +21,7 @@ RoutingThread::~RoutingThread() {
   this->wait();
 }
 
-bool RoutingThread::calculating() { return this->_calculating; }
+bool RoutingThread::calculating() const { return this->_calculating; }
 
 void RoutingThread::route(const snake::BoostPolygon &safeArea,
                           const RoutingThread::Generator &generator) {
@@ -107,7 +107,7 @@ void RoutingThread::run() {
     } // end calculation
 #ifdef DEBUG_CIRCULAR_SURVEY
     else {
-      qWarning() << "RoutingWorker::run(): preconditions failed.";
+      qWarning() << "RoutingWorker::run(): generator() failed.";
     }
 #endif
 #ifdef SHOW_CIRCULAR_SURVEY_TIME

src/Wima/RoutingThread.h

@@ -27,11 +27,12 @@ class RoutingThread : public QThread {
 public:
   using PtrRoutingData = QSharedPointer<RoutingData>;
   using Generator = std::function<bool(snake::Transects &)>;
+  using Consumer = std::function<void(const RoutingData &)>;
 
   RoutingThread(QObject *parent = nullptr);
   ~RoutingThread() override;
 
-  bool calculating();
+  bool calculating() const;
 
 public slots:
   void route(const snake::BoostPolygon &safeArea, const Generator &generator);

src/Wima/Snake/NemoInterface.cpp

 #include "NemoInterface.h"
+#include "SnakeTilesLocal.h"
 
 #include "QGCApplication.h"
 #include "QGCToolbox.h"
@@ -135,12 +136,12 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
         const auto &origin = this->ENUOrigin;
         this->tilesENU.polygons().clear();
         bool error = false;
-        for (std::size_t i = 0; i < tileData.tiles.count(); ++i) {
-          *obj = tileData.tiles.get(i);
-          *tile = qobject_cast<const SnakeTile *>(obj);
+        for (int i = 0; i < tileData.tiles.count(); ++i) {
+          obj = tileData.tiles.get(i);
+          tile = qobject_cast<const SnakeTile *>(obj);
           if (tile != nullptr) {
-            snake::BoostPolygon tileENU;
-            snake::areaToEnu(origin, tile->coordinateList(), tileENU);
+            SnakeTileLocal tileENU;
+            snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
             this->tilesENU.polygons().push_back(std::move(tileENU));
           } else {
             qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
@@ -162,7 +163,7 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
 }
 
 bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
-  return this->tileData = tileData;
+  return this->tileData == tileData;
 }
 
 NemoInterface::NemoStatus NemoInterface::Impl::status() {

src/Wima/Snake/NemoInterface.h

@@ -3,8 +3,6 @@
 #include <QGeoCoordinate>
 #include <QObject>
 
-#include "SnakeTilesLocal.h"
-
 #include <memory>
 
 class TileData;

src/Wima/Snake/SnakeTileLocal.h

 #pragma once
 #include "Wima/Geometry/GenericPolygon.h"
 
-using SnakeTileLocal = GenericPolygon<>;
+using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;

src/Wima/Snake/SnakeTilesLocal.h

 #pragma once
 
-#include "Wima/Snake/SnakeTileLocal.h"
 #include "Wima/Geometry/GenericPolygonArray.h"
-
-typedef GenericPolygonArray<SnakeTileLocal, QVector> SnakeTilesLocal;
+#include "Wima/Snake/SnakeTileLocal.h"
+#include <vector>
+typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;

src/Wima/Snake/snake.h

@@ -92,10 +92,10 @@ struct BoundingBox {
   BoostPolygon corners;
 };
 
-template <class GeoPoint>
-void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
-  GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
-                                  GeographicLib::Constants::WGS84_f());
+template <class GeoPoint1, class GeoPoint2>
+void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, BoostPoint &out) {
+  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
+                                         GeographicLib::Constants::WGS84_f());
   GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
                                      origin.altitude(), earth);
 
@@ -108,10 +108,26 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) {
   (void)z;
 }
 
+template <class GeoPoint1, class GeoPoint2, class Point>
+void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
+  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
+                                         GeographicLib::Constants::WGS84_f());
+  GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
+                                     origin.altitude(), earth);
+
+  double x = 0, y = 0, z = 0;
+  auto alt = in.altitude();
+  alt = std::isnan(alt) ? 0 : alt;
+  proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
+  out.setX(x);
+  out.setY(y);
+  (void)z;
+}
+
 template <class GeoPoint>
 void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
-  GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
-                                  GeographicLib::Constants::WGS84_f());
+  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
+                                         GeographicLib::Constants::WGS84_f());
   GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(),
                                      origin.altitude(), earth);
 
@@ -122,10 +138,18 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) {
   out.setAltitude(alt);
 }
 
-template <class GeoPoint, template <class, class...> class Container>
-void areaToEnu(const GeoPoint &origin, const Container<GeoPoint> &in,
-               BoostPolygon &out) {
-  for (auto vertex : in) {
+template <class GeoPoint, class Container1, class Container2>
+void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
+  for (auto &vertex : in) {
+    typename Container2::value_type p;
+    toENU(origin, vertex, p);
+    out.push_back(p);
+  }
+}
+
+template <class GeoPoint, class Container>
+void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) {
+  for (auto &vertex : in) {
     BoostPoint p;
     toENU(origin, vertex, p);
     out.outer().push_back(p);
@@ -133,11 +157,21 @@ void areaToEnu(const GeoPoint &origin, const Container<GeoPoint> &in,
   bg::correct(out);
 }
 
-template <class GeoPoint, template <class, class...> class Container>
+template <class GeoPoint, class Container1, class Container2>
+void areaFromEnu(const GeoPoint &origin, Container1 &in,
+                 const Container2 &out) {
+  for (auto &vertex : in) {
+    typename Container2::value_type p;
+    fromENU(origin, vertex, p);
+    out.push_back(p);
+  }
+}
+
+template <class GeoPoint, class Container>
 void areaFromEnu(const GeoPoint &origin, BoostPolygon &in,
-                 const Container<GeoPoint> &out) {
-  for (auto vertex : in.outer()) {
-    GeoPoint p;
+                 const Container &out) {
+  for (auto &vertex : in.outer()) {
+    typename Container::value_type p;
     fromENU(origin, vertex, p);
     out.push_back(p);
   }

src/Wima/WimaController.h

@@ -6,12 +6,8 @@
 #include "QGCMapPolygon.h"
 #include "QmlObjectListModel.h"
 
-//#include "Geometry/WimaArea.h"
-//#include "Geometry/WimaCorridor.h"
 #include "Geometry/WimaCorridorData.h"
-//#include "Geometry/WimaMeasurementArea.h"
 #include "Geometry/WimaMeasurementAreaData.h"
-//#include "Geometry/WimaServiceArea.h"
 #include "Geometry/WimaServiceAreaData.h"
 
 #include "WimaPlanData.h"
@@ -33,8 +29,6 @@ typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
 
 class WimaController : public QObject {
   Q_OBJECT
-  enum FileType { WimaFile, PlanFile };
-
 public:
   WimaController(QObject *parent = nullptr);
 
@@ -213,7 +207,7 @@ private slots:
   void _initSmartRTL();
   void _smartRTLCleanUp(bool flying);
   // Snake.
-  void _threadFinishedHandler();
+  void _storeRoute(RoutingThread::PtrRoutingData data);
   void _switchWaypointManager(WaypointManager::ManagerBase &manager);
   void _switchToSnakeWaypointManager(QVariant variant);
   void _progressChangedHandler();
@@ -234,7 +228,7 @@ private:
   WimaMeasurementAreaData _measurementArea; // measurement area
   WimaServiceAreaData _serviceArea;         // area for supplying
   WimaCorridorData _corridor; // corridor connecting opArea and serArea
-  bool _localPlanDataValid;
+  bool _planDataValid;
 
   // Waypoint Managers.
   WaypointManager::AreaInterface _areaInterface;
@@ -283,6 +277,7 @@ private:
   using StatusMap = std::map<int, QString>;
   static StatusMap _nemoStatusMap;
   RoutingThread _routingThread;
+  QGeoCoordinate _origin;
 
   // Periodic tasks.
   QTimer _eventTimer;

src/Wima/WimaPlanData.cc

@@ -89,6 +89,10 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData) {
   }
 }
 
+void WimaPlanData::setTransects(const QList<QList<QGeoCoordinate>> &transects) {
+  _transects = transects;
+}
+
 void WimaPlanData::append(const QList<MissionItem *> &missionItems) {
   for (auto *item : missionItems) {
     item->setParent(this);
@@ -110,6 +114,10 @@ const QList<const WimaAreaData *> &WimaPlanData::areaList() const {
   return _areaList;
 }
 
+const QList<QList<QGeoCoordinate>> &WimaPlanData::transects() const {
+  return _transects;
+}
+
 const QList<MissionItem *> &WimaPlanData::missionItems() const {
   return _missionItems;
 }

src/Wima/WimaPlaner.cc

@@ -668,9 +668,13 @@ void WimaPlaner::synchronize() {
   if (_wimaBridge != nullptr) {
     if (readyForSynchronization()) {
       auto planData = toPlanData();
-      (void)_wimaBridge->setWimaPlanData(planData);
-      this->_synchronized = true;
-      emit synchronizedChanged();
+      if (planData) {
+        (void)_wimaBridge->setWimaPlanData(planData);
+        this->_synchronized = true;
+        emit synchronizedChanged();
+      } else {
+        qWarning("WimaPlaner::uploadToContainer(): error creating plan data.");
+      }
     }
   } else {
     qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
@@ -741,17 +745,18 @@ QSharedPointer<WimaPlanData> WimaPlaner::toPlanData() {
   planData->append(WimaJoinedAreaData(_joinedArea));
 
   // convert mission items to mavlink commands
-  QList<MissionItem *> missionItems;
   if (_missionController && _missionController->visualItems()) {
     int surveyIndex =
         _missionController->visualItems()->indexOf(_TSComplexItem);
     if (surveyIndex > 0) {
+      QList<MissionItem *> missionItems;
       _TSComplexItem->appendMissionItems(missionItems, nullptr);
+      planData->append(missionItems);
+      planData->setTransects(this->_TSComplexItem->rawTransects());
+      return planData;
     }
   }
-  // store mavlink commands
-  planData->append(missionItems);
-  return planData;
+  return QSharedPointer<WimaPlanData>();
 }
 
 #ifndef NDEBUG

src/WimaView/ProgressIndicator.qml

@@ -5,8 +5,8 @@ import QtLocation 5.3
 MapQuickItem {
     id: root
 
-    width: 20
-    height: 20
+    width: 15
+    height: 15
     anchorPoint.x: width/2
     anchorPoint.y: height/2
 
@@ -89,7 +89,7 @@ MapQuickItem {
             // From 0 to angle
 
             ctx.beginPath();
-            ctx.lineWidth = 3;
+            ctx.lineWidth = 1;
             ctx.strokeStyle = root.secondaryColor;
             ctx.arc(root.centerWidth,
                     root.centerHeight,