Add followTerrain test

src/MissionManager/MissionController.cc

@@ -331,7 +331,6 @@ VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordin
             }
         }
     }
-    newItem->setMissionFlightStatus(_missionFlightStatus);
     if (visualItemIndex == -1) {
         _visualItems->append(newItem);
     } else {
@@ -372,7 +371,6 @@ VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate /*coordin
             newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
         }
     }
-    newItem->setMissionFlightStatus(_missionFlightStatus);
     if (visualItemIndex == -1) {
         _visualItems->append(newItem);
     } else {
@@ -1886,9 +1884,10 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
     connect(visualItem, &VisualMissionItem::specifiedVehicleYawChanged,                 this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection);
     connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,                     this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection);
     connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,                 this, &MissionController::_recalcMissionFlightStatusSignal, Qt::QueuedConnection);
-
     connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,                  this, &MissionController::_recalcSequence);
 
+    visualItem->setMissionFlightStatus(_missionFlightStatus);
+
     if (visualItem->isSimpleItem()) {
         // We need to track commandChanged on simple item since recalc has special handling for takeoff command
         SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);

src/MissionManager/TransectStyleComplexItem.cc

@@ -51,7 +51,7 @@ TransectStyleComplexItem::TransectStyleComplexItem(PlanMasterController* masterC
     , _terrainAdjustMaxClimbRateFact    (settingsGroup, _metaDataMap[terrainAdjustMaxClimbRateName])
     , _terrainAdjustMaxDescentRateFact  (settingsGroup, _metaDataMap[terrainAdjustMaxDescentRateName])
 {
-    _terrainQueryTimer.setInterval(_terrainQueryTimeoutMsecs);
+    _terrainQueryTimer.setInterval(qgcApp()->runningUnitTests() ? 0 : _terrainQueryTimeoutMsecs);
     _terrainQueryTimer.setSingleShot(true);
     connect(&_terrainQueryTimer, &QTimer::timeout, this, &TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo);
 

src/MissionManager/TransectStyleComplexItemTest.cc

@@ -211,13 +211,21 @@ void TransectStyleComplexItemTest::_testAltMode(void)
 
 void TransectStyleComplexItemTest::_testFollowTerrain(void) {
     _multiSpy->clearAllSignals();
-    _transectStyleItem->setFollowTerrain(true);
     _transectStyleItem->cameraCalc()->distanceToSurface()->setRawValue(50);
+    _transectStyleItem->setFollowTerrain(true);
     _multiSpy->clearAllSignals();
-    QVERIFY(_multiSpy->waitForSignalByIndex(lastSequenceNumberChangedIndex, 2000));
-    QJsonArray ja;
-    _transectStyleItem->save(ja);
-    qDebug() << ja;
+    while(_transectStyleItem->readyForSaveState() != TransectStyleComplexItem::ReadyForSave) {
+        QVERIFY(_multiSpy->waitForSignalByIndex(lastSequenceNumberChangedIndex, 50));
+    }
+    QList<double> expectedTerrainValues{497,509,512,512};
+    QCOMPARE(_transectStyleItem->transects().size(), 1);
+    for (const auto& transect : _transectStyleItem->transects()) {
+        QCOMPARE(transect.size(), 4);
+        for (const auto pt : transect) {
+            QCOMPARE(pt.coord.altitude(), expectedTerrainValues.front());
+            expectedTerrainValues.pop_front();
+        }
+    }
 }
 
 TestTransectStyleItem::TestTransectStyleItem(PlanMasterController* masterController, QObject* parent)
@@ -228,7 +236,7 @@ TestTransectStyleItem::TestTransectStyleItem(PlanMasterController* masterControl
 {
     // We use a 100m by 100m square test polygon
     const double edgeDistance = 100;
-    surveyAreaPolygon()->appendVertex(QGeoCoordinate(-48.875556, -123.392500));
+    surveyAreaPolygon()->appendVertex(UnitTestTerrainQuery::linearSlopeRegion.center());
     surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(0).atDistanceAndAzimuth(edgeDistance, 90));
     surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(1).atDistanceAndAzimuth(edgeDistance, 180));
     surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(2).atDistanceAndAzimuth(edgeDistance, -90.0));

src/MissionManager/TransectStyleComplexItemTest.h

@@ -96,6 +96,9 @@ public:
     bool recalcComplexDistanceCalled;
     bool recalcCameraShotsCalled;
     void _adjustSurveAreaPolygon(void);
+    QList<QList<CoordInfo_t>> transects() const {
+        return _transects;
+    }
 
 private slots:
     // Overrides from TransectStyleComplexItem

src/Terrain/TerrainQuery.cc

@@ -41,7 +41,7 @@ TerrainAirMapQuery::TerrainAirMapQuery(QObject* parent)
 void TerrainAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit coordinateHeightsReceived(false, QList<double>());
+        UnitTestTerrainQuery(this).requestCoordinateHeights(coordinates);
         return;
     }
 
@@ -62,7 +62,7 @@ void TerrainAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& c
 void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList<double>());
+        UnitTestTerrainQuery(this).requestPathHeights(fromCoord, toCoord);
         return;
     }
 
@@ -82,7 +82,7 @@ void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, con
 void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+        UnitTestTerrainQuery(this).requestCarpetHeights(swCoord, neCoord, statsOnly);
         return;
     }
 
@@ -283,7 +283,7 @@ TerrainOfflineAirMapQuery::TerrainOfflineAirMapQuery(QObject* parent)
 void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit coordinateHeightsReceived(false, QList<double>());
+        UnitTestTerrainQuery(this).requestCoordinateHeights(coordinates);
         return;
     }
 
@@ -297,7 +297,7 @@ void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordin
 void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList<double>());
+        UnitTestTerrainQuery(this).requestPathHeights(fromCoord, toCoord);
         return;
     }
 
@@ -307,7 +307,7 @@ void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoo
 void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+        UnitTestTerrainQuery(this).requestCarpetHeights(swCoord, neCoord, statsOnly);
         return;
     }
 
@@ -802,3 +802,117 @@ void TerrainPolyPathQuery::_terrainDataReceived(bool success, const TerrainPathQ
         _pathQuery.requestData(_rgCoords[_curIndex], _rgCoords[_curIndex+1]);
     }
 }
+
+
+
+const QGeoCoordinate UnitTestTerrainQuery::pointNemo{-48.875556, -123.392500};
+const UnitTestTerrainQuery::Flat10Region UnitTestTerrainQuery::flat10Region{{
+      pointNemo,
+      QGeoCoordinate{
+          pointNemo.latitude() - UnitTestTerrainQuery::regionExtentDeg,
+          pointNemo.longitude() + UnitTestTerrainQuery::regionExtentDeg
+      }
+}};
+const double UnitTestTerrainQuery::Flat10Region::elevationMts;
+
+const UnitTestTerrainQuery::LinearSlopeRegion UnitTestTerrainQuery::linearSlopeRegion{{
+    flat10Region.topRight(),
+    QGeoCoordinate{
+        flat10Region.topRight().latitude() - UnitTestTerrainQuery::regionExtentDeg,
+        flat10Region.topRight().longitude() + UnitTestTerrainQuery::regionExtentDeg
+    }
+}};
+const double UnitTestTerrainQuery::LinearSlopeRegion::minElevationMts;
+const double UnitTestTerrainQuery::LinearSlopeRegion::maxElevationMts;
+const double UnitTestTerrainQuery::LinearSlopeRegion::dElevationMts;
+
+UnitTestTerrainQuery::UnitTestTerrainQuery(TerrainQueryInterface* parent)
+:TerrainQueryInterface(parent)
+{}
+
+void UnitTestTerrainQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates) {
+    QList<double> result = requestCoordinateHeightsSync(coordinates);
+    emit qobject_cast<TerrainQueryInterface*>(parent())->coordinateHeightsReceived(result.size() == coordinates.size(), result);
+}
+
+void UnitTestTerrainQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) {
+    QPair<QList<QGeoCoordinate>, QList<double>> result = requestPathHeightsSync(fromCoord, toCoord);
+    emit qobject_cast<TerrainQueryInterface*>(parent())->pathHeightsReceived(
+        result.second.size() > 0,
+        result.first[0].distanceTo(result.first[1]),
+        result.first[result.first.size()-2].distanceTo(result.first.back()),
+        result.second
+    );
+}
+
+void UnitTestTerrainQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool) {
+    assert(swCoord.longitude() < neCoord.longitude());
+    assert(swCoord.latitude() < neCoord.latitude());
+    double min = std::numeric_limits<double>::max();
+    double max = std::numeric_limits<double>::min();
+    QList<QList<double>> carpet;
+    for (double lat = swCoord.latitude(); lat < neCoord.latitude(); lat++) {
+        QList<double> row = requestPathHeightsSync({lat,swCoord.longitude()}, {lat,neCoord.longitude()}).second;
+        if (row.size() == 0) {
+            emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+            return;
+        }
+        for (const auto val : row) {
+            min = std::min(val,min);
+            max = std::max(val,max);
+        }
+        carpet.push_back(row);
+    }
+    emit qobject_cast<TerrainQueryInterface*>(parent())->carpetHeightsReceived(true, min, max, carpet);
+}
+
+QPair<QList<QGeoCoordinate>, QList<double>> UnitTestTerrainQuery::requestPathHeightsSync(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) {
+    QList<QGeoCoordinate> coordinates;
+    coordinates.push_back(fromCoord);
+
+    //cast to pixels
+    long x0 = fromCoord.longitude()/one_second_deg;
+    long x1 = toCoord.longitude()/one_second_deg;
+    long y0 = fromCoord.latitude()/one_second_deg;
+    long y1 = toCoord.latitude()/one_second_deg;
+
+    //bresenham line algo
+    long dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
+    long dy = abs(y1-y0), sy = y0<y1 ? 1 : -1;
+    long err = (dx>dy ? dx : -dy)/2, e2;
+    while(true) {
+        e2 = err;
+        if (e2 >-dx) { err -= dy; x0 += sx; }
+        if (e2 < dy) { err += dx; y0 += sy; }
+        if ((x0==x1 && y0==y1)) {
+            break;
+        }
+        coordinates.push_back({y0*one_second_deg, x0*one_second_deg});
+    }
+    coordinates.push_back(toCoord);
+    return QPair<QList<QGeoCoordinate>, QList<double>>(coordinates, requestCoordinateHeightsSync(coordinates));
+}
+
+QList<double>  UnitTestTerrainQuery::requestCoordinateHeightsSync(const QList<QGeoCoordinate>& coordinates) {
+    QList<double> result;
+    for (const auto& coordinate : coordinates) {
+        if (flat10Region.contains(coordinate)) {
+            result.push_back(UnitTestTerrainQuery::Flat10Region::elevationMts);
+        } else if (linearSlopeRegion.contains(coordinate)) {
+            //cast to one_second_deg grid and round to int to emulate SRTM1 even better
+            long x = (coordinate.longitude() - linearSlopeRegion.topLeft().longitude())/one_second_deg;
+            long dx = regionExtentDeg/one_second_deg;
+            double fraction = 1.0 * x / dx;
+            result.push_back(
+                std::round(
+                    UnitTestTerrainQuery::LinearSlopeRegion::minElevationMts
+                    + (fraction * UnitTestTerrainQuery::LinearSlopeRegion::dElevationMts)
+                )
+            );
+        } else {
+            result.clear();
+            break;
+        }
+    }
+    return result;
+}

src/Terrain/TerrainQuery.h

@@ -15,6 +15,7 @@
 
 #include <QObject>
 #include <QGeoCoordinate>
+#include <QGeoRectangle>
 #include <QNetworkAccessManager>
 #include <QNetworkReply>
 #include <QTimer>
@@ -296,11 +297,52 @@ private:
 };
 
 ///
-/// \brief The MockTerrainQuery class provides unit test responses for disconnected environment
+/// @brief The MockTerrainQuery class provides unit test terrain query responses for the disconnected environment.
+/// @details It provides preset, emulated, 1 arc-second (SRMT1) resultion regions that are either
+/// flat, sloped or rugged in a fashion that aids testing terrain-sensitive functionality. All emulated
+/// regions are positioned around Point Nemo - should real terrain became useful and checked in one day.
 ///
-class MockTerrainQuery : public TerrainQueryInterface {
+class UnitTestTerrainQuery : public TerrainQueryInterface {
 public:
+
+    static constexpr double regionExtentDeg = 0.1; //every region 0.1deg x 0.1deg across (around 11km north to south)
+    static constexpr double one_second_deg  = 1.0/3600;
+
+    /// @brief Point Nemo is a point on Earth furthest from land
+    static const QGeoCoordinate pointNemo;
+
+    ///
+    /// @brief flat10Region is a region with constant 10m terrain elevation
+    ///
+    struct Flat10Region : public QGeoRectangle {
+        Flat10Region(const QGeoRectangle& region)
+        :QGeoRectangle(region)
+        {}
+
+        static constexpr double elevationMts = 10;
+    };
+    static const Flat10Region flat10Region;
+
+    ///
+    /// @brief linearSlopeRegion is a region with a linear west to east slope raising from -100m to 1000m
+    ///
+    struct LinearSlopeRegion : public QGeoRectangle {
+        LinearSlopeRegion(const QGeoRectangle& region)
+        :QGeoRectangle(region)
+        {}
+
+        static constexpr double minElevationMts = -100;
+        static constexpr double maxElevationMts = 1000;
+        static constexpr double dElevationMts = maxElevationMts-minElevationMts;
+    };
+    static const LinearSlopeRegion linearSlopeRegion;
+
+    UnitTestTerrainQuery(TerrainQueryInterface* parent = nullptr);
+
     void requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates) Q_DECL_OVERRIDE;
     void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) Q_DECL_OVERRIDE;
     void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) Q_DECL_OVERRIDE;
+    QList<double> requestCoordinateHeightsSync(const QList<QGeoCoordinate>& coordinates);
+    QPair<QList<QGeoCoordinate>, QList<double>> requestPathHeightsSync(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord);
 };
+