snake integration continuded

qgroundcontrol.pro

@@ -30,6 +30,7 @@ DebugBuild {
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
+    DEFINES += NDEBUG
 }
 
 #

src/Snake/snake_geometry.h

@@ -68,24 +68,16 @@ public:
     {
         assert((m > 0) || (n > 0));
         assert(!_isInitialized);
-        if (!_isInitialized){
-            _m = m;
-            _n = n;
-            _elements = n*m;
-            _matrix.resize(_elements, value);
-        }
+        _m = m;
+        _n = n;
+        _elements = n*m;
+        _matrix.resize(_elements, value);
+        _isInitialized = true;
     }
 
     void setDimension(size_t m, size_t n)
     {
-        assert((m > 0) || (n > 0));
-        assert(!_isInitialized);
-        if (!_isInitialized){
-            _m = m;
-            _n = n;
-            _elements = n*m;
-            _matrix.resize(_elements);
-        }
+        setDimension(m, n, T{0});
     }
 
 private:

src/Wima/WimaController.cc

@@ -2,6 +2,7 @@
 #include "utilities.h"
 
 #include "time.h"
+#include "assert.h"
 
 
 const char* WimaController::wimaFileExtension           = "wima";
@@ -20,7 +21,7 @@ const char* WimaController::altitudeName                = "Altitude";
 const char* WimaController::reverseName                 = "Reverse";
 const char* WimaController::snakeTileWidthName          = "SnakeTileWidth";
 const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
-const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileAreaWidth";
+const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
 const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
 const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";
 
@@ -87,7 +88,7 @@ WimaController::WimaController(QObject *parent)
     _enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
 
     // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeResultsReady);
+    connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
 }
 
 QmlObjectListModel* WimaController::visualItems()
@@ -1039,6 +1040,7 @@ void WimaController::_eventTimerHandler()
             _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
             _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
             _snakeWorker.start();
+            _setSnakeCalcInProgress(true);
         }
     }
 }
@@ -1311,11 +1313,73 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage()
             errorString.push_back(c);
         qgcApp()->showMessage(errorString);
     }
+    return value;
 }
 
-void WimaController::_snakeResultsReady(const WorkerResult_t &r)
-{
-    // continue here
+void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r)
+{    
+    _setSnakeCalcInProgress(false);
+    if (!r.success) {
+        qgcApp()->showMessage(r.errorMessage);
+        return;
+    }
+
+    // create Mission items from r.waypoints
+    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
+    assert(n >= 1);
+    QVector<MissionItem> missionItems;
+    missionItems.reserve(int(n));
+
+    unsigned long startIdx = r.returnPathIdx.last();
+    unsigned  long endIdx = r.returnPathIdx.first();
+    QObject parent;
+    int seqNum = 0;
+    for (unsigned long i = startIdx; i <= endIdx; ++i) {
+        QGeoCoordinate wp{r.waypoints[int(i)].value<QGeoCoordinate>()};
+        MissionItem item = new MissionItem(seqNum++,
+                                           MAV_CMD_NAV_WAYPOINT,
+                                           MAV_FRAME_GLOBAL_RELATIVE_ALT,
+                                           0,                                           // Hold time (delay for hover and capture to settle vehicle before image is taken)
+                                           0.0,                                         // No acceptance radius specified
+                                           0.0,                                         // Pass through waypoint
+                                           std::numeric_limits<double>::quiet_NaN(),    // Yaw unchanged
+                                           wp.latitude(),
+                                           wp.longitude(),
+                                           wp.altitude(),
+                                           true,                                        // autoContinue
+                                           false,                                       // isCurrentItem
+                                           &parent);
+    }
+
+    // create mission items
+    _missionController->removeAll();
+    QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
+
+    // create SimpleMissionItem by using _missionController
+    for ( int i = 0; i < missionItems.size(); i++) {
+        _missionController->insertSimpleMissionItem(missionItems[i], missionControllerVisualItems->count());
+    }
+    // copy mission items from _missionController to _missionItems
+    for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
+        SimpleMissionItem *visualItem     = qobject_cast<SimpleMissionItem *>((*missionControllerVisualItems)[i]);
+        if (visualItem == nullptr) {
+            qWarning("WimaController::fetchContainerData(): Nullptr at SimpleMissionItem!");
+            return;
+        }
+        SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
+        _missionItems.append(visualItemCopy);
+    }
+
+    _updateWaypointPath();
+
+    // set _nextPhaseStartWaypointIndex to 1
+    disconnect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
+    bool reverse = _reverse.rawValue().toBool();
+    _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1));
+    connect(&_nextPhaseStartWaypointIndex,   &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
+
+    if(!_calcNextPhase())
+        return;
 }
 
 void WimaController::_loadCurrentMissionItemsFromBuffer()

src/Wima/WimaController.h

@@ -232,7 +232,7 @@ private slots:
     void _setSnakeCalcInProgress             (bool inProgress);
     bool _verifyScenarioDefined                    (void);
     bool _verifyScenarioDefinedWithErrorMessage    (void);
-    void _snakeResultsReady                        (const WorkerResult_t &r);
+    void _snakeStoreWorkerResults            (const WorkerResult_t &r);
 
 
 private:
@@ -311,6 +311,7 @@ private:
     SettingsFact                 _snakeMinTileArea;
     SettingsFact                 _snakeLineDistance;
     SettingsFact                 _snakeMinTransectLength;
+    QmlObjectListModel           _snakeTiles; // tiles
 
 
 };

src/main.cc

@@ -58,6 +58,9 @@
 #include <iostream>
 #include "QGCMapEngine.h"
 
+// Snake
+#include "Wima/WimaControllerDetail.h"
+
 /* SDL does ugly things to main() */
 #ifdef main
 #undef main
@@ -145,7 +148,7 @@ int main(int argc, char *argv[])
 
     // install the message handler
     AppMessages::installHandler();
-
+QList<QPair<QByteArray,QByteArray>
 #ifdef Q_OS_MAC
 #ifndef __ios__
     // Prevent Apple's app nap from screwing us over
@@ -250,6 +253,9 @@ int main(int argc, char *argv[])
     // on in the code.
     qRegisterMetaType<QList<QPair<QByteArray,QByteArray> > >();
 
+    // Snake
+    qRegisterMetaType<WorkerResult>("WorkerResult");
+
     app->_initCommon();
     //-- Initialize Cache System
     getQGCMapEngine()->init();