From dc340b1e21f17b38ddfa361022094801d1ccbc03 Mon Sep 17 00:00:00 2001 From: Holden Date: Fri, 6 Sep 2024 21:38:12 -0400 Subject: [PATCH] QtLocationPlugin: Add Elevation Provider Selection --- src/QmlControls/QGroundControlQmlGlobal.cc | 7 +- src/QmlControls/QGroundControlQmlGlobal.h | 4 +- .../Providers/ElevationMapProvider.cpp | 71 +++- .../Providers/ElevationMapProvider.h | 42 ++- src/QtLocationPlugin/QGCCachedTileSet.cpp | 1 - src/QtLocationPlugin/QGCMapUrlEngine.cpp | 13 + src/QtLocationPlugin/QGCMapUrlEngine.h | 1 + .../QMLControl/QGCMapEngineManager.cc | 26 +- .../QMLControl/QGCMapEngineManager.h | 36 ++- src/Settings/FlightMap.SettingsGroup.json | 6 + src/Settings/FlightMapSettings.cc | 1 + src/Settings/FlightMapSettings.h | 2 +- src/Terrain/CMakeLists.txt | 19 +- .../Providers/TerrainQueryArduPilot.cc | 120 +++++++ src/Terrain/Providers/TerrainQueryArduPilot.h | 38 +++ .../Providers/TerrainQueryCopernicus.cc | 213 ++++++++++++ .../TerrainQueryCopernicus.h} | 31 +- src/Terrain/Providers/TerrainTileArduPilot.cc | 46 +++ src/Terrain/Providers/TerrainTileArduPilot.h | 31 ++ .../{ => Providers}/TerrainTileCopernicus.cc | 38 ++- .../{ => Providers}/TerrainTileCopernicus.h | 13 +- src/Terrain/TerrainQuery.cc | 6 +- src/Terrain/TerrainQueryAirMap.cc | 304 ------------------ src/Terrain/TerrainQueryInterface.cc | 113 +++++++ src/Terrain/TerrainQueryInterface.h | 37 +++ src/Terrain/TerrainTile.cc | 26 +- src/Terrain/TerrainTile.h | 8 +- src/Terrain/TerrainTileManager.cc | 143 ++++---- src/Terrain/TerrainTileManager.h | 11 +- src/UI/preferences/MapSettings.qml | 14 + test/Terrain/TerrainQueryTest.cc | 2 +- 31 files changed, 924 insertions(+), 499 deletions(-) create mode 100644 src/Terrain/Providers/TerrainQueryArduPilot.cc create mode 100644 src/Terrain/Providers/TerrainQueryArduPilot.h create mode 100644 src/Terrain/Providers/TerrainQueryCopernicus.cc rename src/Terrain/{TerrainQueryAirMap.h => Providers/TerrainQueryCopernicus.h} (53%) create mode 100644 src/Terrain/Providers/TerrainTileArduPilot.cc create mode 100644 src/Terrain/Providers/TerrainTileArduPilot.h rename src/Terrain/{ => Providers}/TerrainTileCopernicus.cc (89%) rename src/Terrain/{ => Providers}/TerrainTileCopernicus.h (68%) delete mode 100644 src/Terrain/TerrainQueryAirMap.cc diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index ca211f2c04b..a028e57ca0e 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -11,9 +11,10 @@ #include "QGCApplication.h" #include "LinkManager.h" #include "MAVLinkProtocol.h" -#include "ElevationMapProvider.h" #include "FirmwarePluginManager.h" #include "AppSettings.h" +#include "FlightMapSettings.h" +#include "SettingsManager.h" #include "PositionManager.h" #include "QGCMapEngineManager.h" #include "ADSBVehicleManager.h" @@ -335,12 +336,12 @@ int QGroundControlQmlGlobal::mavlinkSystemID() QString QGroundControlQmlGlobal::elevationProviderName() { - return CopernicusElevationProvider::kProviderKey; + return _settingsManager->flightMapSettings()->elevationMapProvider()->rawValue().toString(); } QString QGroundControlQmlGlobal::elevationProviderNotice() { - return CopernicusElevationProvider::kProviderNotice; + return _settingsManager->flightMapSettings()->elevationMapProvider()->rawValue().toString(); } QString QGroundControlQmlGlobal::parameterFileExtension() const diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 609b3ce65b8..e59b13a7714 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -218,8 +218,8 @@ class QGroundControlQmlGlobal : public QGCTool bool hasMAVLinkInspector () { return true; } #endif - static QString elevationProviderName (); - static QString elevationProviderNotice (); + QString elevationProviderName (); + QString elevationProviderNotice (); bool singleFirmwareSupport (); bool singleVehicleSupport (); diff --git a/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp b/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp index 2838caa893a..ec65de42dd9 100644 --- a/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp +++ b/src/QtLocationPlugin/Providers/ElevationMapProvider.cpp @@ -14,27 +14,28 @@ #include "ElevationMapProvider.h" #include "TerrainTileCopernicus.h" +#include "TerrainTileArduPilot.h" int CopernicusElevationProvider::long2tileX(double lon, int z) const { Q_UNUSED(z) - return static_cast(floor((lon + 180.0) / TerrainTileCopernicus::tileSizeDegrees)); + return static_cast(floor((lon + 180.0) / TerrainTileCopernicus::kTileSizeDegrees)); } int CopernicusElevationProvider::lat2tileY(double lat, int z) const { Q_UNUSED(z) - return static_cast(floor((lat + 90.0) / TerrainTileCopernicus::tileSizeDegrees)); + return static_cast(floor((lat + 90.0) / TerrainTileCopernicus::kTileSizeDegrees)); } QString CopernicusElevationProvider::_getURL(int x, int y, int zoom) const { Q_UNUSED(zoom) return _mapUrl - .arg((static_cast(y) * TerrainTileCopernicus::tileSizeDegrees) - 90.0) - .arg((static_cast(x) * TerrainTileCopernicus::tileSizeDegrees) - 180.0) - .arg((static_cast(y + 1) * TerrainTileCopernicus::tileSizeDegrees) - 90.0) - .arg((static_cast(x + 1) * TerrainTileCopernicus::tileSizeDegrees) - 180.0); + .arg((static_cast(y) * TerrainTileCopernicus::kTileSizeDegrees) - 90.0) + .arg((static_cast(x) * TerrainTileCopernicus::kTileSizeDegrees) - 180.0) + .arg((static_cast(y + 1) * TerrainTileCopernicus::kTileSizeDegrees) - 90.0) + .arg((static_cast(x + 1) * TerrainTileCopernicus::kTileSizeDegrees) - 180.0); } QGCTileSet CopernicusElevationProvider::getTileCount(int zoom, double topleftLon, @@ -59,5 +60,61 @@ QGCTileSet CopernicusElevationProvider::getTileCount(int zoom, double topleftLon QByteArray CopernicusElevationProvider::serialize(const QByteArray &image) const { - return TerrainTileCopernicus::serializeFromJson(image); + return TerrainTileCopernicus::serializeFromData(image); +} + +/*===========================================================================*/ + +int ArduPilotTerrainElevationProvider::long2tileX(double lon, int z) const +{ + Q_UNUSED(z) + return static_cast(floor((lon + 180.0))); +} + +int ArduPilotTerrainElevationProvider::lat2tileY(double lat, int z) const +{ + Q_UNUSED(z) + return static_cast(floor((lat + 90.0))); +} + +QString ArduPilotTerrainElevationProvider::_getURL(int x, int y, int zoom) const +{ + Q_UNUSED(zoom) + + // For saving them internally we do 0-360 and 0-180 to avoid signs. Need to redo that to obtain proper format for call + int xForUrl = x - 180; + int yForUrl = y - 90; + + const QString formattedStringYLat = (yForUrl > 0) ? QStringLiteral("N%1").arg(QString::number(yForUrl).rightJustified(2, '0')) : + QStringLiteral("S%1").arg(QString::number(-yForUrl).rightJustified(2, '0')); + + const QString formattedStringXLong = (xForUrl > 0) ? QStringLiteral("E%1").arg(QString::number(xForUrl).rightJustified(3, '0')) : + QStringLiteral("W%1").arg(QString::number(-xForUrl).rightJustified(3, '0')); + + return _mapUrl.arg(formattedStringYLat, formattedStringXLong); +} + +QGCTileSet ArduPilotTerrainElevationProvider::getTileCount(int zoom, double topleftLon, + double topleftLat, double bottomRightLon, + double bottomRightLat) const +{ + QGCTileSet set; + set.tileX0 = long2tileX(topleftLon, zoom); + set.tileY0 = lat2tileY(bottomRightLat, zoom); + set.tileX1 = long2tileX(bottomRightLon, zoom); + set.tileY1 = lat2tileY(topleftLat, zoom); + + set.tileCount = (static_cast(set.tileX1) - + static_cast(set.tileX0) + 1) * + (static_cast(set.tileY1) - + static_cast(set.tileY0) + 1); + + set.tileSize = getAverageSize() * set.tileCount; + + return set; +} + +QByteArray ArduPilotTerrainElevationProvider::serialize(const QByteArray &image) const +{ + return TerrainTileArduPilot::serializeFromData(image); } diff --git a/src/QtLocationPlugin/Providers/ElevationMapProvider.h b/src/QtLocationPlugin/Providers/ElevationMapProvider.h index eae1a39918d..8a3f281ede3 100644 --- a/src/QtLocationPlugin/Providers/ElevationMapProvider.h +++ b/src/QtLocationPlugin/Providers/ElevationMapProvider.h @@ -2,8 +2,6 @@ #include "MapProvider.h" -static constexpr const quint32 AVERAGE_COPERNICUS_ELEV_SIZE = 2786; - class ElevationProvider : public MapProvider { protected: @@ -21,13 +19,14 @@ class ElevationProvider : public MapProvider virtual QByteArray serialize(const QByteArray &image) const = 0; }; +/// https://spacedata.copernicus.eu/collections/copernicus-digital-elevation-model class CopernicusElevationProvider : public ElevationProvider { public: CopernicusElevationProvider() : ElevationProvider( - QStringLiteral("Copernicus Elevation"), - QStringLiteral("https://terrain-ce.suite.auterion.com/"), + kProviderKey, + kProviderURL, QStringLiteral("bin"), AVERAGE_COPERNICUS_ELEV_SIZE, QGeoMapType::StreetMap) {} @@ -43,9 +42,42 @@ class CopernicusElevationProvider : public ElevationProvider static constexpr const char *kProviderKey = "Copernicus Elevation"; static constexpr const char *kProviderNotice = "© Airbus Defence and Space GmbH"; + static constexpr const char *kProviderURL = "https://terrain-ce.suite.auterion.com"; + static constexpr quint32 AVERAGE_COPERNICUS_ELEV_SIZE = 2786; + +private: + QString _getURL(int x, int y, int zoom) const final; + + const QString _mapUrl = QString(kProviderURL) + QStringLiteral("/api/v1/carpet?points=%1,%2,%3,%4"); +}; + +class ArduPilotTerrainElevationProvider : public ElevationProvider +{ +public: + ArduPilotTerrainElevationProvider() + : ElevationProvider( + kProviderKey, + kProviderURL, + QStringLiteral("hgt"), + AVERAGE_ARDUPILOT_ELEV_SIZE, + QGeoMapType::StreetMap) {} + + int long2tileX(double lon, int z) const final; + int lat2tileY(double lat, int z) const final; + + QGCTileSet getTileCount(int zoom, double topleftLon, + double topleftLat, double bottomRightLon, + double bottomRightLat) const final; + + QByteArray serialize(const QByteArray &image) const final; + + static constexpr const char *kProviderKey = "ArduPilot Terrain Elevation"; + static constexpr const char *kProviderNotice = "© ArduPilot.org"; + static constexpr const char *kProviderURL = "https://terrain.ardupilot.org/SRTM1"; + static constexpr quint32 AVERAGE_ARDUPILOT_ELEV_SIZE = 100000; private: QString _getURL(int x, int y, int zoom) const final; - const QString _mapUrl = QStringLiteral("https://terrain-ce.suite.auterion.com/api/v1/carpet?points=%1,%2,%3,%4"); + const QString _mapUrl = QString(kProviderURL) + QStringLiteral("/%1%2.hgt.zip"); }; diff --git a/src/QtLocationPlugin/QGCCachedTileSet.cpp b/src/QtLocationPlugin/QGCCachedTileSet.cpp index 0bbd7ae8555..0421cbef8d5 100644 --- a/src/QtLocationPlugin/QGCCachedTileSet.cpp +++ b/src/QtLocationPlugin/QGCCachedTileSet.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index d2620113864..a4092d997c3 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -63,6 +63,7 @@ const QList UrlFactory::_providers = { std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), std::make_shared(), @@ -229,6 +230,18 @@ int UrlFactory::getQtMapIdFromProviderType(QStringView type) return -1; } +QStringList UrlFactory::getElevationProviderTypes() +{ + QStringList types; + for (const SharedMapProvider &provider : _providers) { + if (provider->isElevationProvider()) { + (void) types.append(provider->getMapName()); + } + } + + return types; +} + QStringList UrlFactory::getProviderTypes() { QStringList types; diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.h b/src/QtLocationPlugin/QGCMapUrlEngine.h index 63161b413d4..d0713f3b165 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.h +++ b/src/QtLocationPlugin/QGCMapUrlEngine.h @@ -45,6 +45,7 @@ class UrlFactory QStringView mapType); static const QList>& getProviders() { return _providers; } + static QStringList getElevationProviderTypes(); static QStringList getProviderTypes(); static int getQtMapIdFromProviderType(QStringView type); static QString getProviderTypeFromQtMapId(int qtMapId); diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index d49e0a46a14..f3398732173 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -18,6 +18,8 @@ #include "ElevationMapProvider.h" #include "QmlObjectListModel.h" #include "QGCApplication.h" +#include "QGCToolbox.h" +#include "SettingsManager.h" #include "QGCLoggingCategory.h" #include @@ -28,9 +30,6 @@ QGC_LOGGING_CATEGORY(QGCMapEngineManagerLog, "qgc.qtlocation.qmlcontrol.qgcmapenginemanagerlog") -static const QString kQmlOfflineMapKeyName = QStringLiteral("QGCOfflineMap"); -static const QString kElevationMapType = QString(CopernicusElevationProvider::kProviderKey); // TODO: Variable Elevation Provider Type - Q_APPLICATION_STATIC(QGCMapEngineManager, _mapEngineManager); QGCMapEngineManager *QGCMapEngineManager::instance() @@ -76,7 +75,8 @@ void QGCMapEngineManager::updateForCurrentView(double lon0, double lat0, double } if (_fetchElevation) { - const QGCTileSet set = UrlFactory::getTileCount(1, lon0, lat0, lon1, lat1, kElevationMapType); + const QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + const QGCTileSet set = UrlFactory::getTileCount(1, lon0, lat0, lon1, lat1, elevationProviderName); _elevationSet += set; } @@ -143,9 +143,11 @@ void QGCMapEngineManager::startDownload(const QString &name, const QString &mapT qCWarning(QGCMapEngineManagerLog) << Q_FUNC_INFO << "No Tiles to save"; } - if ((mapType != kElevationMapType) && _fetchElevation) { + const int mapid = UrlFactory::getQtMapIdFromProviderType(mapType); + if (_fetchElevation && !UrlFactory::isElevation(mapid)) { QGCCachedTileSet* const set = new QGCCachedTileSet(name + QStringLiteral(" Elevation")); - set->setMapTypeStr(kElevationMapType); + const QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + set->setMapTypeStr(elevationProviderName); set->setTopleftLat(_topleftLat); set->setTopleftLon(_topleftLon); set->setBottomRightLat(_bottomRightLat); @@ -154,7 +156,7 @@ void QGCMapEngineManager::startDownload(const QString &name, const QString &mapT set->setMaxZoom(1); set->setTotalTileSize(_elevationSet.tileSize); set->setTotalTileCount(static_cast(_elevationSet.tileCount)); - set->setType(kElevationMapType); + set->setType(elevationProviderName); QGCCreateTileSetTask* const task = new QGCCreateTileSetTask(set); (void) connect(task, &QGCCreateTileSetTask::tileSetSaved, this, &QGCMapEngineManager::_tileSetSaved); @@ -458,7 +460,10 @@ QStringList QGCMapEngineManager::mapList() QStringList QGCMapEngineManager::mapProviderList() { QStringList mapStringList = mapList(); - (void) mapStringList.removeAll(CopernicusElevationProvider::kProviderKey); + const QStringList elevationStringList = elevationProviderList(); + for (const QString &elevationProviderName : elevationStringList) { + (void) mapStringList.removeAll(elevationProviderName); + } static const QRegularExpression providerType = QRegularExpression(QStringLiteral("^([^\\ ]*) (.*)$")); (void) mapStringList.replaceInStrings(providerType,"\\1"); @@ -466,3 +471,8 @@ QStringList QGCMapEngineManager::mapProviderList() return mapStringList; } + +QStringList QGCMapEngineManager::elevationProviderList() +{ + return UrlFactory::getElevationProviderTypes(); +} diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h index b7c930620a7..ca2d82f7794 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h @@ -32,21 +32,22 @@ class QGCMapEngineManager : public QObject Q_MOC_INCLUDE("QmlObjectListModel.h") Q_MOC_INCLUDE("QGCCachedTileSet.h") - Q_PROPERTY(bool fetchElevation MEMBER _fetchElevation NOTIFY fetchElevationChanged) - Q_PROPERTY(bool importReplace MEMBER _importReplace NOTIFY importReplaceChanged) - Q_PROPERTY(ImportAction importAction READ importAction WRITE setImportAction NOTIFY importActionChanged) - Q_PROPERTY(int actionProgress READ actionProgress NOTIFY actionProgressChanged) - Q_PROPERTY(int selectedCount READ selectedCount NOTIFY selectedCountChanged) - Q_PROPERTY(QmlObjectListModel *tileSets READ tileSets NOTIFY tileSetsChanged) - Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) - Q_PROPERTY(QString tileCountStr READ tileCountStr NOTIFY tileCountChanged) - Q_PROPERTY(QString tileSizeStr READ tileSizeStr NOTIFY tileSizeChanged) - Q_PROPERTY(QStringList mapList READ mapList CONSTANT) - Q_PROPERTY(QStringList mapProviderList READ mapProviderList CONSTANT) - Q_PROPERTY(quint32 diskSpace READ diskSpace) - Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) - Q_PROPERTY(quint64 tileCount READ tileCount NOTIFY tileCountChanged) - Q_PROPERTY(quint64 tileSize READ tileSize NOTIFY tileSizeChanged) + Q_PROPERTY(bool fetchElevation MEMBER _fetchElevation NOTIFY fetchElevationChanged) + Q_PROPERTY(bool importReplace MEMBER _importReplace NOTIFY importReplaceChanged) + Q_PROPERTY(ImportAction importAction READ importAction WRITE setImportAction NOTIFY importActionChanged) + Q_PROPERTY(int actionProgress READ actionProgress NOTIFY actionProgressChanged) + Q_PROPERTY(int selectedCount READ selectedCount NOTIFY selectedCountChanged) + Q_PROPERTY(QmlObjectListModel *tileSets READ tileSets NOTIFY tileSetsChanged) + Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) + Q_PROPERTY(QString tileCountStr READ tileCountStr NOTIFY tileCountChanged) + Q_PROPERTY(QString tileSizeStr READ tileSizeStr NOTIFY tileSizeChanged) + Q_PROPERTY(QStringList mapList READ mapList CONSTANT) + Q_PROPERTY(QStringList mapProviderList READ mapProviderList CONSTANT) + Q_PROPERTY(QStringList elevationProviderList READ elevationProviderList CONSTANT) + Q_PROPERTY(quint32 diskSpace READ diskSpace) + Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) + Q_PROPERTY(quint64 tileCount READ tileCount NOTIFY tileCountChanged) + Q_PROPERTY(quint64 tileSize READ tileSize NOTIFY tileSizeChanged) public: QGCMapEngineManager(QObject *parent = nullptr); @@ -97,6 +98,7 @@ class QGCMapEngineManager : public QObject static QStringList mapList(); static QStringList mapProviderList(); + static QStringList elevationProviderList(); signals: void actionProgressChanged(); @@ -123,7 +125,7 @@ private slots: void _updateTotals(quint32 totaltiles, quint64 totalsize, quint32 defaulttiles, quint64 defaultsize); private: - void _updateDiskFreeSpace(); + void _updateDiskFreeSpace(); QmlObjectListModel *_tileSets = nullptr; QGCTileSet _imageSet; @@ -142,4 +144,6 @@ private slots: QString _errorMessage; bool _fetchElevation = true; bool _importReplace = false; + + static constexpr const char *kQmlOfflineMapKeyName = "QGCOfflineMap"; }; diff --git a/src/Settings/FlightMap.SettingsGroup.json b/src/Settings/FlightMap.SettingsGroup.json index 1c0746207aa..5686a02aa6c 100644 --- a/src/Settings/FlightMap.SettingsGroup.json +++ b/src/Settings/FlightMap.SettingsGroup.json @@ -14,6 +14,12 @@ "shortDesc": "Currently selected map type for flight maps", "type": "string", "default": "Hybrid" +}, +{ + "name": "elevationMapProvider", + "shortDesc": "Currently selected elevation map provider", + "type": "string", + "default": "Copernicus Elevation" } ] } diff --git a/src/Settings/FlightMapSettings.cc b/src/Settings/FlightMapSettings.cc index 902392cbfe2..2e361b19c06 100644 --- a/src/Settings/FlightMapSettings.cc +++ b/src/Settings/FlightMapSettings.cc @@ -18,3 +18,4 @@ DECLARE_SETTINGGROUP(FlightMap, "FlightMap") DECLARE_SETTINGSFACT(FlightMapSettings, mapProvider) DECLARE_SETTINGSFACT(FlightMapSettings, mapType) +DECLARE_SETTINGSFACT(FlightMapSettings, elevationMapProvider) diff --git a/src/Settings/FlightMapSettings.h b/src/Settings/FlightMapSettings.h index 6627b3fe2d7..fb9ef5298e1 100644 --- a/src/Settings/FlightMapSettings.h +++ b/src/Settings/FlightMapSettings.h @@ -22,5 +22,5 @@ class FlightMapSettings : public SettingsGroup DEFINE_SETTING_NAME_GROUP() DEFINE_SETTINGFACT(mapProvider) DEFINE_SETTINGFACT(mapType) - + DEFINE_SETTINGFACT(elevationMapProvider) }; diff --git a/src/Terrain/CMakeLists.txt b/src/Terrain/CMakeLists.txt index f2e477af8c4..435ac0d3319 100644 --- a/src/Terrain/CMakeLists.txt +++ b/src/Terrain/CMakeLists.txt @@ -1,16 +1,20 @@ find_package(Qt6 REQUIRED COMPONENTS Core Location Network Positioning) qt_add_library(Terrain STATIC + Providers/TerrainQueryArduPilot.cc + Providers/TerrainQueryArduPilot.h + Providers/TerrainQueryCopernicus.cc + Providers/TerrainQueryCopernicus.h + Providers/TerrainTileArduPilot.cc + Providers/TerrainTileArduPilot.h + Providers/TerrainTileCopernicus.cc + Providers/TerrainTileCopernicus.h TerrainQuery.cc TerrainQuery.h - TerrainQueryAirMap.cc - TerrainQueryAirMap.h TerrainQueryInterface.cc TerrainQueryInterface.h TerrainTile.cc TerrainTile.h - TerrainTileCopernicus.cc - TerrainTileCopernicus.h TerrainTileManager.cc TerrainTileManager.h ) @@ -18,6 +22,7 @@ qt_add_library(Terrain STATIC target_link_libraries(Terrain PRIVATE Qt6::LocationPrivate + Compression QGCLocation Utilities PUBLIC @@ -26,4 +31,8 @@ target_link_libraries(Terrain Qt6::Positioning ) -target_include_directories(Terrain PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories(Terrain + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/Providers +) diff --git a/src/Terrain/Providers/TerrainQueryArduPilot.cc b/src/Terrain/Providers/TerrainQueryArduPilot.cc new file mode 100644 index 00000000000..358ef8b73e8 --- /dev/null +++ b/src/Terrain/Providers/TerrainQueryArduPilot.cc @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainQueryArduPilot.h" +#include "TerrainTileArduPilot.h" +#include "ElevationMapProvider.h" +#include "QGCFileDownload.h" +#include "QGCLoggingCategory.h" +#include "QGCMapUrlEngine.h" +#include "QGCZip.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainQueryArduPilotLog, "qgc.terrain.terrainqueryardupilot") + +TerrainQueryArduPilot::TerrainQueryArduPilot(QObject *parent) + : TerrainOnlineQuery(parent) +{ + // qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << this; +} + +TerrainQueryArduPilot::~TerrainQueryArduPilot() +{ + // qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << this; +} + +void TerrainQueryCopernicus::requestCarpetHeights(const QGeoCoordinate &swCoord, const QGeoCoordinate &neCoord, bool statsOnly) +{ + if (!swCoord.isValid() || !neCoord.isValid()) { + return; + } + + _queryMode = TerrainQuery::QueryModeCarpet; + _carpetStatsOnly = statsOnly; + + const SharedMapProvider provider = UrlFactory::getMapProviderFromProviderType(QString(ArduPilotTerrainElevationProvider::kProviderKey)); + // getTileCount + + // const int x = provider->long2tileX(coord.longitude(), 1); + // const int y = provider->lat2tileY(coord.latitude(), 1); + // const QUrl url = provider->getTileURL(x, y, 1); + // _sendQuery(url); +} + +void TerrainQueryArduPilot::_sendQuery(const QUrl &url) +{ + qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << url; + + QGCFileDownload *download = new QGCFileDownload(this); + (void) connect(download, &QGCFileDownload::downloadComplete, this, &TerrainQueryArduPilot::_parseZipFile); + if (!download->download(url.toString())) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Download File"; + _requestFailed(); + } +} + +void TerrainQueryArduPilot::_parseZipFile(QString remoteFile, QString localFile, QString errorMsg) +{ + if (!errorMsg.isEmpty()) { + qCWarning(TerrainQueryArduPilotLog) << "Error During Download:" << errorMsg; + _requestFailed(); + return; + } + + const QString outputDirectoryPath = QDir(QStandardPaths::writableLocation(QStandardPaths::TempLocation)).path() + "/SRTM1"; + if (!QGCZip::unzipFile(localFile, outputDirectoryPath)) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Unzip File" << localFile; + _requestFailed(); + return; + } + + /*QFile file(outputDirectoryPath + "/" + remoteFile); + if (!file.open(QIODevice::ReadOnly)) { + qCWarning(TerrainQueryArduPilotLog) << "Failed To Unzip File" << localFile; + _requestFailed(); + return; + } + + qCDebug(TerrainQueryArduPilotLog) << Q_FUNC_INFO << "success"; + switch (_queryMode) { + case TerrainQuery::QueryModeCoordinates: + _parseCoordinateData(responseBytes); + break; + default: + qCWarning(TerrainQueryArduPilotLog) << Q_FUNC_INFO << "Query Mode Not Supported"; + break; + }*/ +} + +void TerrainQueryArduPilot::_parseCarpetData(const QByteArray &data) +{ + Q_UNUSED(data); + + // QList rawElevationData(TerrainTileArduPilot::kTotalPoints); + // rawElevationData = reinterpret_cast>(data.constData()); + + // QList> elevationData(TerrainTileArduPilot::kTileDimension); + // for (int row = 0; row < TerrainTileArduPilot::kTileDimension; ++row) { + // elevationData[row].resize(TerrainTileArduPilot::kTileDimension); + // for (int col = 0; col < TerrainTileArduPilot::kTileDimension; ++col) { + // elevationData[row][col] = rawElevationData[row * TerrainTileArduPilot::kTileDimension + col]; + // } + // } + + // emit coordinateHeightsReceived(true, elevationData); +} diff --git a/src/Terrain/Providers/TerrainQueryArduPilot.h b/src/Terrain/Providers/TerrainQueryArduPilot.h new file mode 100644 index 00000000000..babf7e7dc5b --- /dev/null +++ b/src/Terrain/Providers/TerrainQueryArduPilot.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include "TerrainQueryInterface.h" + +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryArduPilotLog) + +class QGeoCoordinate; + + +class TerrainQueryArduPilot : public TerrainOnlineQuery +{ + Q_OBJECT + +public: + explicit TerrainQueryArduPilot(QObject *parent = nullptr); + ~TerrainQueryArduPilot(); + + void requestCarpetHeights(const QGeoCoordinate &swCoord, const QGeoCoordinate &neCoord, bool statsOnly) final; + +private: + void _sendQuery(const QUrl &path); + void _parseCarpetData(const QByteArray &coordinateData); + void _parseZipFile(QString remoteFile, QString localFile, QString errorMsg); + + bool _carpetStatsOnly = false; +}; diff --git a/src/Terrain/Providers/TerrainQueryCopernicus.cc b/src/Terrain/Providers/TerrainQueryCopernicus.cc new file mode 100644 index 00000000000..b9910c0678f --- /dev/null +++ b/src/Terrain/Providers/TerrainQueryCopernicus.cc @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainQueryCopernicus.h" +#include "TerrainTileCopernicus.h" +#include "ElevationMapProvider.h" +#include "QGCFileDownload.h" +#include "QGCLoggingCategory.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainQueryCopernicusLog, "qgc.terrain.terrainquerycopernicus") + +TerrainQueryCopernicus::TerrainQueryCopernicus(QObject *parent) + : TerrainOnlineQuery(parent) +{ + // qCDebug(TerrainQueryCopernicusLog) << Q_FUNC_INFO << this; +} + +TerrainQueryCopernicus::~TerrainQueryCopernicus() +{ + // qCDebug(TerrainQueryCopernicusLog) << Q_FUNC_INFO << this; +} + +void TerrainQueryCopernicus::requestCoordinateHeights(const QList &coordinates) +{ + if (coordinates.isEmpty()) { + return; + } + + QString points; + for (const QGeoCoordinate &coord: coordinates) { + const QString point = QString::number(coord.latitude(), 'f', 10) + "," + QString::number(coord.longitude(), 'f', 10); + points += (point + ","); + } + points = points.mid(0, points.length() - 1); // remove the last ',' from string + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = TerrainQuery::QueryModeCoordinates; + _sendQuery(QString(), query); +} + +void TerrainQueryCopernicus::requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) +{ + QString points; + points += QString::number(fromCoord.latitude(), 'f', 10) + "," + + QString::number(fromCoord.longitude(), 'f', 10) + ","; + points += QString::number(toCoord.latitude(), 'f', 10) + "," + + QString::number(toCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = TerrainQuery::QueryModePath; + _sendQuery(QStringLiteral("/path"), query); +} + +void TerrainQueryCopernicus::requestCarpetHeights(const QGeoCoordinate &swCoord, const QGeoCoordinate &neCoord, bool statsOnly) +{ + QString points; + points += QString::number(swCoord.latitude(), 'f', 10) + "," + + QString::number(swCoord.longitude(), 'f', 10) + ","; + points += QString::number(neCoord.latitude(), 'f', 10) + "," + + QString::number(neCoord.longitude(), 'f', 10); + + QUrlQuery query; + query.addQueryItem(QStringLiteral("points"), points); + + _queryMode = TerrainQuery::QueryModeCarpet; + _carpetStatsOnly = statsOnly; + + _sendQuery(QStringLiteral("/carpet"), query); +} + +void TerrainQueryCopernicus::_sendQuery(const QString &path, const QUrlQuery &urlQuery) +{ + // const SharedMapProvider provider = UrlFactory::getMapProviderFromProviderType(CopernicusElevationProvider::kProviderKey); + // provider->getTileURL() + + QUrl url(QString(CopernicusElevationProvider::kProviderURL) + QStringLiteral("/api/v1") + path); + url.setQuery(urlQuery); + qCDebug(TerrainQueryCopernicusLog) << Q_FUNC_INFO << url; + + QNetworkRequest request(url); + QSslConfiguration sslConf = request.sslConfiguration(); + sslConf.setPeerVerifyMode(QSslSocket::VerifyNone); + request.setSslConfiguration(sslConf); + + QNetworkReply* const networkReply = _networkManager->get(request); + if (!networkReply) { + qCWarning(TerrainQueryCopernicusLog) << "QNetworkManager::Get did not return QNetworkReply"; + _requestFailed(); + return; + } + + QGCFileDownload::setIgnoreSSLErrorsIfNeeded(*networkReply); + + (void) connect(networkReply, &QNetworkReply::finished, this, &TerrainQueryCopernicus::_requestFinished); + (void) connect(networkReply, &QNetworkReply::sslErrors, this, &TerrainQueryCopernicus::_sslErrors); + (void) connect(networkReply, &QNetworkReply::errorOccurred, this, &TerrainQueryCopernicus::_requestError); +} + +void TerrainQueryCopernicus::_requestFinished() +{ + QNetworkReply* const reply = qobject_cast(QObject::sender()); + if (!reply) { + qCWarning(TerrainQueryCopernicusLog) << Q_FUNC_INFO << "null reply"; + return; + } + + if (reply->error() != QNetworkReply::NoError) { + qCWarning(TerrainQueryCopernicusLog) << Q_FUNC_INFO << "error:url:data" << reply->error() << reply->url() << reply->readAll(); + reply->deleteLater(); + _requestFailed(); + return; + } + + const QByteArray responseBytes = reply->readAll(); + reply->deleteLater(); + + const QJsonValue &jsonData = TerrainTileCopernicus::getJsonFromData(responseBytes); + if (jsonData.isNull()) { + _requestFailed(); + return; + } + + qCDebug(TerrainQueryCopernicusLog) << Q_FUNC_INFO << "success"; + switch (_queryMode) { + case TerrainQuery::QueryModeCoordinates: + _parseCoordinateData(jsonData); + break; + case TerrainQuery::QueryModePath: + _parsePathData(jsonData); + break; + case TerrainQuery::QueryModeCarpet: + _parseCarpetData(jsonData); + break; + default: + break; + } +} + +void TerrainQueryCopernicus::_parseCoordinateData(const QJsonValue &coordinateJson) +{ + const QJsonArray dataArray = coordinateJson.toArray(); + + QList heights; + for (const QJsonValue &dataValue: dataArray) { + (void) heights.append(dataValue.toDouble()); + } + + emit coordinateHeightsReceived(true, heights); +} + +void TerrainQueryCopernicus::_parsePathData(const QJsonValue &pathJson) +{ + const QJsonObject jsonObject = pathJson.toArray()[0].toObject(); + const QJsonArray stepArray = jsonObject["step"].toArray(); + const QJsonArray profileArray = jsonObject["profile"].toArray(); + + const double latStep = stepArray[0].toDouble(); + const double lonStep = stepArray[1].toDouble(); + + QList heights; + for (const QJsonValue &profileValue: profileArray) { + (void) heights.append(profileValue.toDouble()); + } + + emit pathHeightsReceived(true, latStep, lonStep, heights); +} + +void TerrainQueryCopernicus::_parseCarpetData(const QJsonValue &carpetJson) +{ + const QJsonObject jsonObject = carpetJson.toArray()[0].toObject(); + + const QJsonObject statsObject = jsonObject["stats"].toObject(); + const double minHeight = statsObject["min"].toDouble(); + const double maxHeight = statsObject["max"].toDouble(); + + QList> carpet; + if (!_carpetStatsOnly) { + const QJsonArray carpetArray = jsonObject["carpet"].toArray(); + + for (qsizetype i = 0; i < carpetArray.count(); i++) { + const QJsonArray rowArray = carpetArray[i].toArray(); + (void) carpet.append(QList()); + + for (qsizetype j = 0; j < rowArray.count(); j++) { + const double height = rowArray[j].toDouble(); + (void) carpet.last().append(height); + } + } + } + + emit carpetHeightsReceived(true, minHeight, maxHeight, carpet); +} diff --git a/src/Terrain/TerrainQueryAirMap.h b/src/Terrain/Providers/TerrainQueryCopernicus.h similarity index 53% rename from src/Terrain/TerrainQueryAirMap.h rename to src/Terrain/Providers/TerrainQueryCopernicus.h index 0f63b7ad0e0..5078e3f0056 100644 --- a/src/Terrain/TerrainQueryAirMap.h +++ b/src/Terrain/Providers/TerrainQueryCopernicus.h @@ -11,54 +11,33 @@ #include #include -#include #include "TerrainQueryInterface.h" -Q_DECLARE_LOGGING_CATEGORY(TerrainQueryAirMapLog) -Q_DECLARE_LOGGING_CATEGORY(TerrainQueryAirMapVerboseLog) +Q_DECLARE_LOGGING_CATEGORY(TerrainQueryCopernicusLog) class QGeoCoordinate; -class QNetworkAccessManager; -class TerrainAirMapQuery : public TerrainQueryInterface +class TerrainQueryCopernicus : public TerrainOnlineQuery { Q_OBJECT public: - explicit TerrainAirMapQuery(QObject *parent = nullptr); - ~TerrainAirMapQuery(); + explicit TerrainQueryCopernicus(QObject *parent = nullptr); + ~TerrainQueryCopernicus(); void requestCoordinateHeights(const QList &coordinates) final; void requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) final; void requestCarpetHeights(const QGeoCoordinate &swCoord, const QGeoCoordinate &neCoord, bool statsOnly) final; private slots: - void _requestError(QNetworkReply::NetworkError code); - void _requestFinished(); - void _sslErrors(const QList &errors); + void _requestFinished() final; private: void _sendQuery(const QString &path, const QUrlQuery &urlQuery); - void _requestFailed(); void _parseCoordinateData(const QJsonValue &coordinateJson); void _parsePathData(const QJsonValue &pathJson); void _parseCarpetData(const QJsonValue &carpetJson); bool _carpetStatsOnly = false; - QNetworkAccessManager *_networkManager = nullptr; -}; - -/*===========================================================================*/ - -class TerrainOfflineAirMapQuery : public TerrainQueryInterface -{ - Q_OBJECT - -public: - explicit TerrainOfflineAirMapQuery(QObject *parent = nullptr); - ~TerrainOfflineAirMapQuery(); - - void requestCoordinateHeights(const QList &coordinates) final; - void requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) final; }; diff --git a/src/Terrain/Providers/TerrainTileArduPilot.cc b/src/Terrain/Providers/TerrainTileArduPilot.cc new file mode 100644 index 00000000000..c8cbc5eb480 --- /dev/null +++ b/src/Terrain/Providers/TerrainTileArduPilot.cc @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TerrainTileArduPilot.h" +#include "JsonHelper.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(TerrainTileArduPilotLog, "qgc.terrain.terraintileardupilot"); + +TerrainTileArduPilot::TerrainTileArduPilot() +{ + // qCDebug(TerrainTileArduPilotLog) << Q_FUNC_INFO << this; +} + +TerrainTileArduPilot::TerrainTileArduPilot(const QByteArray &byteArray) + : TerrainTile(byteArray) +{ + // qCDebug(TerrainTileArduPilotLog) << Q_FUNC_INFO << this; +} + +TerrainTileArduPilot::~TerrainTileArduPilot() +{ + // qCDebug(TerrainTileArduPilotLog) << Q_FUNC_INFO << this; +} + +QByteArray TerrainTileArduPilot::serializeFromData(const QByteArray &input) +{ + TerrainTile::TileInfo_t tileInfo; + + constexpr int cTileNumHeaderBytes = static_cast(sizeof(TileInfo_t)); + const int cTileNumDataBytes = static_cast(sizeof(int16_t)) * tileInfo.gridSizeLat * tileInfo.gridSizeLon; + + QByteArray result(cTileNumHeaderBytes + cTileNumDataBytes, Qt::Uninitialized); + TileInfo_t *pTileInfo = reinterpret_cast(result.data()); + int16_t* const pTileData = reinterpret_cast(&reinterpret_cast(result.data())[cTileNumHeaderBytes]); + + *pTileInfo = tileInfo; + + return result; +} diff --git a/src/Terrain/Providers/TerrainTileArduPilot.h b/src/Terrain/Providers/TerrainTileArduPilot.h new file mode 100644 index 00000000000..6bc10c7d13a --- /dev/null +++ b/src/Terrain/Providers/TerrainTileArduPilot.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +#include "TerrainTile.h" + +Q_DECLARE_LOGGING_CATEGORY(TerrainTileArduPilotLog) + +class TerrainTileArduPilot : public TerrainTile +{ +public: + TerrainTileArduPilot(); + + /// Constructor from serialized elevation data (either from file or web) + /// @param byteArray + explicit TerrainTileArduPilot(const QByteArray &byteArray); + ~TerrainTileArduPilot(); + + static QByteArray serializeFromData(const QByteArray &input); + + /// SRTM-1 (1 arc-second resolution, 30 meters): 3601 × 3601 grid of elevations (129,672,001 data points). 0-84 degrees N/S. + static constexpr double kTileSizeDegrees = 0.01; ///< Each terrain tile represents a square area .01 degrees in lat/lon + static constexpr double kTileValueSpacingDegrees = (1.0 / 3600); ///< 1 Arc-Second spacing of elevation values + static constexpr double kTileValueSpacingMeters = 30.0; ///< SRTM1 Resolution + static constexpr double kTileDimension = 3601; ///< SRTM1 Total Points + static constexpr double kTotalPoints = pow(kTileDimension, 2); ///< SRTM1 Total Points + + /// SRTM-3 (3 arc-second resolution, 90 meters): 1201 × 1201 grid of elevations (1,442,401 data points). 0-84 degrees N/S. >60N & <60S filled from SRTM1 + // static constexpr double tileValueSpacingMeters = 90.0; ///< SRTM3 Resolution +}; diff --git a/src/Terrain/TerrainTileCopernicus.cc b/src/Terrain/Providers/TerrainTileCopernicus.cc similarity index 89% rename from src/Terrain/TerrainTileCopernicus.cc rename to src/Terrain/Providers/TerrainTileCopernicus.cc index a5a6ffe5920..c5f10366210 100644 --- a/src/Terrain/TerrainTileCopernicus.cc +++ b/src/Terrain/Providers/TerrainTileCopernicus.cc @@ -33,18 +33,18 @@ TerrainTileCopernicus::~TerrainTileCopernicus() // qCDebug(TerrainTileCopernicusLog) << Q_FUNC_INFO << this; } -QByteArray TerrainTileCopernicus::serializeFromJson(const QByteArray &input) +QJsonValue TerrainTileCopernicus::getJsonFromData(const QByteArray &input) { QJsonParseError parseError; const QJsonDocument document = QJsonDocument::fromJson(input, &parseError); if (parseError.error != QJsonParseError::NoError) { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Terrain tile json doc parse error" << parseError.errorString(); - return QByteArray(); + return QJsonValue(); } if (!document.isObject()) { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Terrain tile json doc is no object"; - return QByteArray(); + return QJsonValue(); } const QJsonObject rootObject = document.object(); @@ -56,30 +56,46 @@ QByteArray TerrainTileCopernicus::serializeFromJson(const QByteArray &input) QString errorString; if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Error in reading json: " << errorString; - return QByteArray(); + return QJsonValue(); } if (rootObject[_jsonStatusKey].toString() != "success") { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Invalid terrain tile."; - return QByteArray(); + return QJsonValue(); } - const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject(); + const QJsonValue &jsonData = rootObject[_jsonDataKey]; + const QJsonObject &dataObject = jsonData.toObject(); + QList dataVersionKeyInfoList = { { _jsonBoundsKey, QJsonValue::Object, true }, { _jsonStatsKey, QJsonValue::Object, true }, { _jsonCarpetKey, QJsonValue::Array, true }, }; + if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Error in reading json: " << errorString; + return QJsonValue(); + } + + return jsonData; +} + +QByteArray TerrainTileCopernicus::serializeFromData(const QByteArray &input) +{ + const QJsonValue &jsonData = getJsonFromData(input); + if (jsonData.isNull()) { return QByteArray(); } + const QJsonObject &dataObject = jsonData.toObject(); + const QJsonObject &boundsObject = dataObject[_jsonBoundsKey].toObject(); static const QList boundsVersionKeyInfoList = { { _jsonSouthWestKey, QJsonValue::Array, true }, { _jsonNorthEastKey, QJsonValue::Array, true }, }; + QString errorString; if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) { qCWarning(TerrainTileCopernicusLog) << Q_FUNC_INFO << "Error in reading json: " << errorString; return QByteArray(); @@ -92,14 +108,6 @@ QByteArray TerrainTileCopernicus::serializeFromJson(const QByteArray &input) return QByteArray(); } - const double swLat = swArray[0].toDouble(); - const double swLon = swArray[1].toDouble(); - const double neLat = neArray[0].toDouble(); - const double neLon = neArray[1].toDouble(); - - qCDebug(TerrainTileCopernicusLog) << "Serialize: swArray: south west:" << (40.42 - swLat) << (-3.23 - swLon); - qCDebug(TerrainTileCopernicusLog) << "Serialize: neArray: north east:" << neLat << neLon; - const QJsonObject &statsObject = dataObject[_jsonStatsKey].toObject(); static const QList statsVersionKeyInfoList = { { _jsonMinElevationKey, QJsonValue::Double, true }, @@ -127,7 +135,7 @@ QByteArray TerrainTileCopernicus::serializeFromJson(const QByteArray &input) qCDebug(TerrainTileCopernicusLog) << "Serialize: TileInfo: south west:" << tileInfo.swLat << tileInfo.swLon; qCDebug(TerrainTileCopernicusLog) << "Serialize: TileInfo: north east:" << tileInfo.neLat << tileInfo.neLon; - const int cTileNumHeaderBytes = static_cast(sizeof(TileInfo_t)); + constexpr int cTileNumHeaderBytes = static_cast(sizeof(TileInfo_t)); const int cTileNumDataBytes = static_cast(sizeof(int16_t)) * tileInfo.gridSizeLat * tileInfo.gridSizeLon; QByteArray result(cTileNumHeaderBytes + cTileNumDataBytes, Qt::Uninitialized); diff --git a/src/Terrain/TerrainTileCopernicus.h b/src/Terrain/Providers/TerrainTileCopernicus.h similarity index 68% rename from src/Terrain/TerrainTileCopernicus.h rename to src/Terrain/Providers/TerrainTileCopernicus.h index d5894a73fd0..a3946a14f59 100644 --- a/src/Terrain/TerrainTileCopernicus.h +++ b/src/Terrain/Providers/TerrainTileCopernicus.h @@ -7,22 +7,23 @@ Q_DECLARE_LOGGING_CATEGORY(TerrainTileCopernicusLog) -/// Implements an interface for https://developers.airmap.com/v2.0/docs/elevation-api +/// Implements an interface for https://terrain-ce.suite.auterion.com/api/v1/ class TerrainTileCopernicus : public TerrainTile { public: TerrainTileCopernicus(); /// Constructor from serialized elevation data (either from file or web) - /// @param document + /// @param byteArray explicit TerrainTileCopernicus(const QByteArray &byteArray); ~TerrainTileCopernicus(); - static QByteArray serializeFromJson(const QByteArray &input); + static QByteArray serializeFromData(const QByteArray &input); + static QJsonValue getJsonFromData(const QByteArray &input); - static constexpr double tileSizeDegrees = 0.01; ///< Each terrain tile represents a square area .01 degrees in lat/lon - static constexpr double tileValueSpacingDegrees = (1.0 / 3600); ///< 1 Arc-Second spacing of elevation values - static constexpr double tileValueSpacingMeters = 30.0; + static constexpr double kTileSizeDegrees = 0.01; ///< Each terrain tile represents a square area .01 degrees in lat/lon + static constexpr double kTleValueSpacingDegrees = (1.0 / 3600); ///< 1 Arc-Second spacing of elevation values + static constexpr double kTileValueSpacingMeters = 30.0; private: static constexpr const char *_jsonStatusKey = "status"; diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 268ab115c1c..53f55552d76 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -8,7 +8,7 @@ ****************************************************************************/ #include "TerrainQuery.h" -#include "TerrainQueryAirMap.h" +#include "TerrainQueryInterface.h" #include "TerrainTileManager.h" #include "QGCLoggingCategory.h" @@ -22,7 +22,7 @@ Q_GLOBAL_STATIC(TerrainAtCoordinateBatchManager, _terrainAtCoordinateBatchManage TerrainAtCoordinateBatchManager::TerrainAtCoordinateBatchManager(QObject *parent) : QObject(parent) , _batchTimer(new QTimer(this)) - , _terrainQuery(new TerrainOfflineAirMapQuery(this)) + , _terrainQuery(new TerrainOfflineQuery(this)) { // qCDebug(TerrainQueryLog) << Q_FUNC_INFO << this; @@ -225,7 +225,7 @@ void TerrainAtCoordinateQuery::signalTerrainData(bool success, const QList - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "TerrainQueryAirMap.h" -#include "TerrainTileManager.h" -#include "QGCFileDownload.h" -#include "QGCLoggingCategory.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -QGC_LOGGING_CATEGORY(TerrainQueryAirMapLog, "qgc.terrain.terrainqueryairmap") -QGC_LOGGING_CATEGORY(TerrainQueryAirMapVerboseLog, "qgc.terrain.terrainqueryairmap.verbose") - -TerrainAirMapQuery::TerrainAirMapQuery(QObject *parent) - : TerrainQueryInterface(parent) - , _networkManager(new QNetworkAccessManager(this)) -{ - // qCDebug(TerrainAirMapQuery) << Q_FUNC_INFO << this; - -#if defined(Q_OS_ANDROID) || defined(Q_OS_IOS) - QNetworkProxy proxy = _networkManager->proxy(); - proxy.setType(QNetworkProxy::DefaultProxy); - _networkManager->setProxy(proxy); -#endif - - qCDebug(TerrainQueryAirMapVerboseLog) << "supportsSsl" << QSslSocket::supportsSsl() << "sslLibraryBuildVersionString" << QSslSocket::sslLibraryBuildVersionString(); -} - -TerrainAirMapQuery::~TerrainAirMapQuery() -{ - // qCDebug(TerrainAirMapQuery) << Q_FUNC_INFO << this; -} - -void TerrainAirMapQuery::requestCoordinateHeights(const QList &coordinates) -{ - if (coordinates.isEmpty()) { - return; - } - - QString points; - for (const QGeoCoordinate &coord: coordinates) { - points += QString::number(coord.latitude(), 'f', 10) + "," - + QString::number(coord.longitude(), 'f', 10) + ","; - } - points = points.mid(0, points.length() - 1); // remove the last ',' from string - - QUrlQuery query; - query.addQueryItem(QStringLiteral("points"), points); - - _queryMode = TerrainQuery::QueryModeCoordinates; - _sendQuery(QString(), query); -} - -void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) -{ - QString points; - points += QString::number(fromCoord.latitude(), 'f', 10) + "," - + QString::number(fromCoord.longitude(), 'f', 10) + ","; - points += QString::number(toCoord.latitude(), 'f', 10) + "," - + QString::number(toCoord.longitude(), 'f', 10); - - QUrlQuery query; - query.addQueryItem(QStringLiteral("points"), points); - - _queryMode = TerrainQuery::QueryModePath; - _sendQuery(QStringLiteral("/path"), query); -} - -void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate &swCoord, const QGeoCoordinate &neCoord, bool statsOnly) -{ - QString points; - points += QString::number(swCoord.latitude(), 'f', 10) + "," - + QString::number(swCoord.longitude(), 'f', 10) + ","; - points += QString::number(neCoord.latitude(), 'f', 10) + "," - + QString::number(neCoord.longitude(), 'f', 10); - - QUrlQuery query; - query.addQueryItem(QStringLiteral("points"), points); - - _queryMode = TerrainQuery::QueryModeCarpet; - _carpetStatsOnly = statsOnly; - - _sendQuery(QStringLiteral("/carpet"), query); -} - -void TerrainAirMapQuery::_sendQuery(const QString &path, const QUrlQuery &urlQuery) -{ - // TODO: Invalid URL - // https://terrain-ce.suite.auterion.com/api/v1/ - QUrl url(QStringLiteral("https://api.airmap.com/elevation/v1/ele") + path); - qCDebug(TerrainQueryAirMapLog) << Q_FUNC_INFO << url; - url.setQuery(urlQuery); - - QNetworkRequest request(url); - QSslConfiguration sslConf = request.sslConfiguration(); - sslConf.setPeerVerifyMode(QSslSocket::VerifyNone); - request.setSslConfiguration(sslConf); - - QNetworkReply* const networkReply = _networkManager->get(request); - if (!networkReply) { - qCWarning(TerrainQueryAirMapLog) << "QNetworkManager::Get did not return QNetworkReply"; - _requestFailed(); - return; - } - - QGCFileDownload::setIgnoreSSLErrorsIfNeeded(*networkReply); - - (void) connect(networkReply, &QNetworkReply::finished, this, &TerrainAirMapQuery::_requestFinished); - (void) connect(networkReply, &QNetworkReply::sslErrors, this, &TerrainAirMapQuery::_sslErrors); - (void) connect(networkReply, &QNetworkReply::errorOccurred, this, &TerrainAirMapQuery::_requestError); -} - -void TerrainAirMapQuery::_requestError(QNetworkReply::NetworkError code) -{ - if (code != QNetworkReply::NoError) { - QNetworkReply* const reply = qobject_cast(QObject::sender()); - qCWarning(TerrainQueryAirMapLog) << Q_FUNC_INFO << "error:url:data" << reply->error() << reply->url() << reply->readAll(); - } -} - -void TerrainAirMapQuery::_sslErrors(const QList &errors) -{ - for (const QSslError &error : errors) { - qCWarning(TerrainQueryAirMapLog) << "SSL error:" << error.errorString(); - - const QSslCertificate &certificate = error.certificate(); - if (!certificate.isNull()) { - qCWarning(TerrainQueryAirMapLog) << "SSL Certificate problem:" << certificate.toText(); - } - } -} - -void TerrainAirMapQuery::_requestFinished() -{ - QNetworkReply* const reply = qobject_cast(QObject::sender()); - if (!reply) { - qCWarning(TerrainQueryAirMapLog) << Q_FUNC_INFO << "null reply"; - return; - } - - if (reply->error() != QNetworkReply::NoError) { - qCWarning(TerrainQueryAirMapLog) << Q_FUNC_INFO << "error:url:data" << reply->error() << reply->url() << reply->readAll(); - reply->deleteLater(); - _requestFailed(); - return; - } - - const QByteArray responseBytes = reply->readAll(); - reply->deleteLater(); - - // Convert the response to Json - QJsonParseError parseError; - const QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCWarning(TerrainQueryAirMapLog) << Q_FUNC_INFO << "unable to parse json:" << parseError.errorString(); - _requestFailed(); - return; - } - - // Check airmap reponse status - const QJsonObject rootObject = responseJson.object(); - const QString status = rootObject["status"].toString(); - if (status != "success") { - qCWarning(TerrainQueryAirMapLog) << Q_FUNC_INFO << "status != success:" << status; - _requestFailed(); - return; - } - - // Send back data - const QJsonValue &jsonData = rootObject["data"]; - qCDebug(TerrainQueryAirMapLog) << Q_FUNC_INFO << "success"; - switch (_queryMode) { - case TerrainQuery::QueryModeCoordinates: - _parseCoordinateData(jsonData); - break; - case TerrainQuery::QueryModePath: - _parsePathData(jsonData); - break; - case TerrainQuery::QueryModeCarpet: - _parseCarpetData(jsonData); - break; - default: - break; - } -} - -void TerrainAirMapQuery::_requestFailed() -{ - switch (_queryMode) { - case TerrainQuery::QueryModeCoordinates: - emit coordinateHeightsReceived(false, QList()); - break; - case TerrainQuery::QueryModePath: - emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList()); - break; - case TerrainQuery::QueryModeCarpet: - emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList>()); - break; - default: - break; - } -} - -void TerrainAirMapQuery::_parseCoordinateData(const QJsonValue &coordinateJson) -{ - const QJsonArray dataArray = coordinateJson.toArray(); - - QList heights; - for (const QJsonValue &dataValue: dataArray) { - (void) heights.append(dataValue.toDouble()); - } - - emit coordinateHeightsReceived(true, heights); -} - -void TerrainAirMapQuery::_parsePathData(const QJsonValue &pathJson) -{ - const QJsonObject jsonObject = pathJson.toArray()[0].toObject(); - const QJsonArray stepArray = jsonObject["step"].toArray(); - const QJsonArray profileArray = jsonObject["profile"].toArray(); - - const double latStep = stepArray[0].toDouble(); - const double lonStep = stepArray[1].toDouble(); - - QList heights; - for (const QJsonValue &profileValue: profileArray) { - (void) heights.append(profileValue.toDouble()); - } - - emit pathHeightsReceived(true, latStep, lonStep, heights); -} - -void TerrainAirMapQuery::_parseCarpetData(const QJsonValue &carpetJson) -{ - const QJsonObject jsonObject = carpetJson.toArray()[0].toObject(); - - const QJsonObject statsObject = jsonObject["stats"].toObject(); - const double minHeight = statsObject["min"].toDouble(); - const double maxHeight = statsObject["max"].toDouble(); - - QList> carpet; - if (!_carpetStatsOnly) { - const QJsonArray carpetArray = jsonObject["carpet"].toArray(); - - for (qsizetype i = 0; i < carpetArray.count(); i++) { - const QJsonArray rowArray = carpetArray[i].toArray(); - (void) carpet.append(QList()); - - for (qsizetype j = 0; j < rowArray.count(); j++) { - const double height = rowArray[j].toDouble(); - (void) carpet.last().append(height); - } - } - } - - emit carpetHeightsReceived(true, minHeight, maxHeight, carpet); -} - -/*===========================================================================*/ - -TerrainOfflineAirMapQuery::TerrainOfflineAirMapQuery(QObject *parent) - : TerrainQueryInterface(parent) -{ - // qCDebug(AudioOutputLog) << Q_FUNC_INFO << this; - - qCDebug(TerrainQueryAirMapVerboseLog) << "supportsSsl" << QSslSocket::supportsSsl() << "sslLibraryBuildVersionString" << QSslSocket::sslLibraryBuildVersionString(); -} - -TerrainOfflineAirMapQuery::~TerrainOfflineAirMapQuery() -{ - // qCDebug(AudioOutputLog) << Q_FUNC_INFO << this; -} - -void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList &coordinates) -{ - if (coordinates.isEmpty()) { - return; - } - - _queryMode = TerrainQuery::QueryModeCoordinates; - TerrainTileManager::instance()->addCoordinateQuery(this, coordinates); -} - -void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) -{ - _queryMode = TerrainQuery::QueryModePath; - TerrainTileManager::instance()->addPathQuery(this, fromCoord, toCoord); -} diff --git a/src/Terrain/TerrainQueryInterface.cc b/src/Terrain/TerrainQueryInterface.cc index 2f1c3b3dedd..33d128bc72a 100644 --- a/src/Terrain/TerrainQueryInterface.cc +++ b/src/Terrain/TerrainQueryInterface.cc @@ -8,8 +8,11 @@ ****************************************************************************/ #include "TerrainQueryInterface.h" +#include "TerrainTileManager.h" #include "QGCLoggingCategory.h" +#include +#include #include QGC_LOGGING_CATEGORY(TerrainQueryInterfaceLog, "qgc.terrain.terrainqueryinterface") @@ -49,3 +52,113 @@ void TerrainQueryInterface::signalCarpetHeights(bool success, double minHeight, { emit carpetHeightsReceived(success, minHeight, maxHeight, carpet); } + +void TerrainQueryInterface::_requestFailed() +{ + switch (_queryMode) { + case TerrainQuery::QueryModeCoordinates: + emit coordinateHeightsReceived(false, QList()); + break; + case TerrainQuery::QueryModePath: + emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList()); + break; + case TerrainQuery::QueryModeCarpet: + emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList>()); + break; + default: + qCWarning(TerrainQueryInterfaceLog) << Q_FUNC_INFO << "Query Mode Not Supported"; + break; + } +} + +/*===========================================================================*/ + +TerrainOfflineQuery::TerrainOfflineQuery(QObject *parent) + : TerrainQueryInterface(parent) +{ + // qCDebug(TerrainQueryInterfaceLog) << Q_FUNC_INFO << this; +} + +TerrainOfflineQuery::~TerrainOfflineQuery() +{ + // qCDebug(TerrainQueryInterfaceLog) << Q_FUNC_INFO << this; +} + +void TerrainOfflineQuery::requestCoordinateHeights(const QList &coordinates) +{ + if (coordinates.isEmpty()) { + return; + } + + _queryMode = TerrainQuery::QueryModeCoordinates; + TerrainTileManager::instance()->addCoordinateQuery(this, coordinates); +} + +void TerrainOfflineQuery::requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) +{ + _queryMode = TerrainQuery::QueryModePath; + TerrainTileManager::instance()->addPathQuery(this, fromCoord, toCoord); +} + +/*===========================================================================*/ + +TerrainOnlineQuery::TerrainOnlineQuery(QObject *parent) + : TerrainQueryInterface(parent) + , _networkManager(new QNetworkAccessManager(this)) +{ + // qCDebug(TerrainQueryInterfaceLog) << Q_FUNC_INFO << this; + + qCDebug(TerrainQueryInterfaceLog) << "supportsSsl" << QSslSocket::supportsSsl() << "sslLibraryBuildVersionString" << QSslSocket::sslLibraryBuildVersionString(); + +#if defined(Q_OS_ANDROID) || defined(Q_OS_IOS) + QNetworkProxy proxy = _networkManager->proxy(); + proxy.setType(QNetworkProxy::DefaultProxy); + _networkManager->setProxy(proxy); +#endif +} + +TerrainOnlineQuery::~TerrainOnlineQuery() +{ + // qCDebug(TerrainQueryInterfaceLog) << Q_FUNC_INFO << this; +} + +void TerrainOnlineQuery::_requestFinished() +{ + QNetworkReply* const reply = qobject_cast(QObject::sender()); + if (!reply) { + qCWarning(TerrainQueryInterfaceLog) << Q_FUNC_INFO << "null reply"; + return; + } + + if (reply->error() != QNetworkReply::NoError) { + qCWarning(TerrainQueryInterfaceLog) << Q_FUNC_INFO << "error:url:data" << reply->error() << reply->url() << reply->readAll(); + reply->deleteLater(); + _requestFailed(); + return; + } + + const QByteArray responseBytes = reply->readAll(); + reply->deleteLater(); + + qCDebug(TerrainQueryInterfaceLog) << Q_FUNC_INFO << "success"; +} + +void TerrainOnlineQuery::_requestError(QNetworkReply::NetworkError code) +{ + if (code != QNetworkReply::NoError) { + QNetworkReply* const reply = qobject_cast(QObject::sender()); + qCWarning(TerrainQueryInterfaceLog) << Q_FUNC_INFO << "error:url:data" << reply->error() << reply->url() << reply->readAll(); + } +} + +void TerrainOnlineQuery::_sslErrors(const QList &errors) +{ + for (const QSslError &error : errors) { + qCWarning(TerrainQueryInterfaceLog) << "SSL error:" << error.errorString(); + + const QSslCertificate &certificate = error.certificate(); + if (!certificate.isNull()) { + qCWarning(TerrainQueryInterfaceLog) << "SSL Certificate problem:" << certificate.toText(); + } + } +} diff --git a/src/Terrain/TerrainQueryInterface.h b/src/Terrain/TerrainQueryInterface.h index fc025e8858d..cb00c8fceab 100644 --- a/src/Terrain/TerrainQueryInterface.h +++ b/src/Terrain/TerrainQueryInterface.h @@ -3,8 +3,10 @@ #include #include #include +#include class QGeoCoordinate; +class QNetworkAccessManager; Q_DECLARE_LOGGING_CATEGORY(TerrainQueryInterfaceLog) @@ -58,5 +60,40 @@ class TerrainQueryInterface : public QObject void carpetHeightsReceived(bool success, double minHeight, double maxHeight, const QList> &carpet); protected: + virtual void _requestFailed(); + TerrainQuery::QueryMode _queryMode = TerrainQuery::QueryMode::QueryModeNone; }; + +/*===========================================================================*/ + +class TerrainOfflineQuery : public TerrainQueryInterface +{ + Q_OBJECT + +public: + explicit TerrainOfflineQuery(QObject *parent = nullptr); + ~TerrainOfflineQuery(); + + void requestCoordinateHeights(const QList &coordinates) override; + void requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) override; +}; + +/*===========================================================================*/ + +class TerrainOnlineQuery : public TerrainQueryInterface +{ + Q_OBJECT + +public: + explicit TerrainOnlineQuery(QObject *parent = nullptr); + ~TerrainOnlineQuery(); + +protected slots: + virtual void _requestFinished(); + virtual void _requestError(QNetworkReply::NetworkError code); + virtual void _sslErrors(const QList &errors); + +protected: + QNetworkAccessManager *_networkManager = nullptr; +}; diff --git a/src/Terrain/TerrainTile.cc b/src/Terrain/TerrainTile.cc index 2cadd3c89bc..3a5d5aa863c 100644 --- a/src/Terrain/TerrainTile.cc +++ b/src/Terrain/TerrainTile.cc @@ -25,6 +25,14 @@ TerrainTile::TerrainTile(const QByteArray &byteArray) { // qCDebug(TerrainTileLog) << Q_FUNC_INFO << this; + constexpr int cTileHeaderBytes = static_cast(sizeof(TileInfo_t)); + const int cTileBytesAvailable = byteArray.size(); + + if (cTileBytesAvailable < cTileHeaderBytes) { + qCWarning(TerrainTileLog) << "Terrain tile binary data too small for TileInfo_s header"; + return; + } + if (((_tileInfo.neLon - _tileInfo.swLon) < 0.0) || ((_tileInfo.neLat - _tileInfo.swLat) < 0.0)) { qCWarning(TerrainTileLog) << this << "Tile extent is infeasible"; _isValid = false; @@ -40,20 +48,6 @@ TerrainTile::TerrainTile(const QByteArray &byteArray) qCDebug(TerrainTileLog) << this << "TileInfo: min, max, avg:" << _tileInfo.minElevation << _tileInfo.maxElevation << _tileInfo.avgElevation; qCDebug(TerrainTileLog) << this << "TileInfo: cell size:" << _cellSizeLat << _cellSizeLon; - const int cTileHeaderBytes = static_cast(sizeof(TileInfo_t)); - const int cTileBytesAvailable = byteArray.size(); - - if (cTileBytesAvailable < cTileHeaderBytes) { - qCWarning(TerrainTileLog) << "Terrain tile binary data too small for TileInfo_s header"; - return; - } - - const int cTileDataBytes = static_cast(sizeof(int16_t)) * _tileInfo.gridSizeLat * _tileInfo.gridSizeLon; - if (cTileBytesAvailable < cTileHeaderBytes + cTileDataBytes) { - qCWarning(TerrainTileLog) << "Terrain tile binary data too small for tile data"; - return; - } - _elevationData.resize(_tileInfo.gridSizeLat); for (int k = 0; k < _tileInfo.gridSizeLat; k++) { _elevationData[k].resize(_tileInfo.gridSizeLon); @@ -98,8 +92,8 @@ double TerrainTile::elevation(const QGeoCoordinate &coordinate) const if ((latIndex >= _elevationData.size()) || (lonIndex >= _elevationData[latIndex].size())) { qCWarning(TerrainTileLog).noquote() << this << "Internal error: _elevationData size inconsistent _tileInfo << coordinate" << coordinate - << "\n\t_tillIndo.gridSizeLat: " << _tileInfo.gridSizeLat << " _tileInfo.gridSizeLon: " << _tileInfo.gridSizeLon - << "\n\t_data.size(): " << _elevationData.size() << " _elevationData[latIndex].size(): " << _elevationData[latIndex].size(); + << "\n\t_tillIndo.gridSizeLat:" << _tileInfo.gridSizeLat << "_tileInfo.gridSizeLon:" << _tileInfo.gridSizeLon + << "\n\t_data.size():" << _elevationData.size() << "_elevationData[latIndex].size():" << _elevationData[latIndex].size(); return qQNaN(); } const auto elevation = _elevationData[latIndex][lonIndex]; diff --git a/src/Terrain/TerrainTile.h b/src/Terrain/TerrainTile.h index 9bb96531ac2..ab6375794d0 100644 --- a/src/Terrain/TerrainTile.h +++ b/src/Terrain/TerrainTile.h @@ -48,8 +48,8 @@ class TerrainTile private: TileInfo_t _tileInfo{}; - QList> _elevationData; /// 2D elevation data array - double _cellSizeLat = 0.0; /// data grid size in latitude direction - double _cellSizeLon = 0.0; /// data grid size in longitude direction - bool _isValid = false; /// data loaded is valid + QList> _elevationData; ///< 2D elevation data array + double _cellSizeLat = 0.0; ///< data grid size in latitude direction + double _cellSizeLon = 0.0; ///< data grid size in longitude direction + bool _isValid = false; ///< data loaded is valid }; diff --git a/src/Terrain/TerrainTileManager.cc b/src/Terrain/TerrainTileManager.cc index aceaa327b2c..9bef06975d0 100644 --- a/src/Terrain/TerrainTileManager.cc +++ b/src/Terrain/TerrainTileManager.cc @@ -10,11 +10,12 @@ #include "TerrainTileManager.h" #include "TerrainTile.h" #include "TerrainTileCopernicus.h" -// #include "TerrainQueryAirMap.h" #include "QGeoTileFetcherQGC.h" #include "QGeoMapReplyQGC.h" #include "QGCMapUrlEngine.h" #include "ElevationMapProvider.h" +#include "QGCApplication.h" +#include "SettingsManager.h" #include "QGCLoggingCategory.h" #include @@ -51,6 +52,51 @@ TerrainTileManager::~TerrainTileManager() // qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << this; } +bool TerrainTileManager::getAltitudesForCoordinates(const QList &coordinates, QList &altitudes, bool &error) +{ + error = false; + + const QString elevationProviderName = qgcApp()->toolbox()->settingsManager()->flightMapSettings()->elevationMapProvider()->rawValue().toString(); + const SharedMapProvider provider = UrlFactory::getMapProviderFromProviderType(elevationProviderName); + for (const QGeoCoordinate &coordinate: coordinates) { + const QString tileHash = UrlFactory::getTileHash( + provider->getMapName(), + provider->long2tileX(coordinate.longitude(), 1), + provider->lat2tileY(coordinate.latitude(), 1), + 1 + ); + qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "hash:coordinate" << tileHash << coordinate; + + TerrainTile* const tile = _getCachedTile(tileHash); + if (tile) { + const double elevation = tile->elevation(coordinate); + if (qIsNaN(elevation)) { + error = true; + qCWarning(TerrainTileManagerLog) << Q_FUNC_INFO << "Internal Error: missing elevation in tile cache"; + } else { + qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "returning elevation from tile cache" << elevation; + } + altitudes.push_back(elevation); + } else if (_state != TerrainQuery::State::Downloading) { + QGeoTileSpec spec; + spec.setX(provider->long2tileX(coordinate.longitude(), 1)); + spec.setY(provider->lat2tileY(coordinate.latitude(), 1)); + spec.setZoom(1); + spec.setMapId(provider->getMapId()); + const QNetworkRequest request = QGeoTileFetcherQGC::getNetworkRequest(spec.mapId(), spec.x(), spec.y(), spec.zoom()); + QGeoTiledMapReplyQGC* const reply = new QGeoTiledMapReplyQGC(_networkManager, request, spec, this); + (void) connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainTileManager::_terrainDone); + _state = TerrainQuery::State::Downloading; + // TODO: Batch Downloading? + return false; + } else { + return false; + } + } + + return true; +} + void TerrainTileManager::addCoordinateQuery(TerrainQueryInterface *terrainQueryInterface, const QList &coordinates) { qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "count" << coordinates.count(); @@ -85,42 +131,11 @@ void TerrainTileManager::addCoordinateQuery(TerrainQueryInterface *terrainQueryI terrainQueryInterface->signalCoordinateHeights((coordinates.count() == altitudes.count()), altitudes); } -QList TerrainTileManager::pathQueryToCoords(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord, double &distanceBetween, double &finalDistanceBetween) -{ - const double lat = fromCoord.latitude(); - const double lon = fromCoord.longitude(); - const int steps = qCeil(toCoord.distanceTo(fromCoord) / TerrainTileCopernicus::tileValueSpacingMeters); - const double latDiff = toCoord.latitude() - lat; - const double lonDiff = toCoord.longitude() - lon; - - QList coordinates; - if (steps == 0) { - (void) coordinates.append(fromCoord); - (void) coordinates.append(toCoord); - distanceBetween = finalDistanceBetween = coordinates[0].distanceTo(coordinates[1]); - } else { - for (int i = 0; i <= steps; i++) { - const double latStep = lat + ((latDiff * static_cast(i)) / static_cast(steps)); - const double lonStep = lon + ((lonDiff * static_cast(i)) / static_cast(steps)); - (void) coordinates.append(QGeoCoordinate(latStep, lonStep)); - } - - // We always have one too many and we always want the last one to be the endpoint - coordinates.last() = toCoord; - distanceBetween = coordinates[0].distanceTo(coordinates[1]); - finalDistanceBetween = coordinates[coordinates.count() - 2].distanceTo(coordinates.last()); - } - - qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "fromCoord:toCoord:distanceBetween:finalDisanceBetween:coordCount" << fromCoord << toCoord << distanceBetween << finalDistanceBetween << coordinates.count(); - - return coordinates; -} - void TerrainTileManager::addPathQuery(TerrainQueryInterface *terrainQueryInterface, const QGeoCoordinate &startPoint, const QGeoCoordinate &endPoint) { double distanceBetween; double finalDistanceBetween; - const QList coordinates = pathQueryToCoords(startPoint, endPoint, distanceBetween, finalDistanceBetween); + const QList coordinates = _pathQueryToCoords(startPoint, endPoint, distanceBetween, finalDistanceBetween); bool error; QList altitudes; @@ -148,49 +163,35 @@ void TerrainTileManager::addPathQuery(TerrainQueryInterface *terrainQueryInterfa terrainQueryInterface->signalPathHeights((coordinates.count() == altitudes.count()), distanceBetween, finalDistanceBetween, altitudes); } -bool TerrainTileManager::getAltitudesForCoordinates(const QList &coordinates, QList &altitudes, bool &error) +QList TerrainTileManager::_pathQueryToCoords(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord, double &distanceBetween, double &finalDistanceBetween) { - error = false; - - static const QString kMapType = CopernicusElevationProvider::kProviderKey; - const SharedMapProvider provider = UrlFactory::getMapProviderFromProviderType(kMapType); - for (const QGeoCoordinate &coordinate: coordinates) { - const QString tileHash = UrlFactory::getTileHash( - provider->getMapName(), - provider->long2tileX(coordinate.longitude(), 1), - provider->lat2tileY(coordinate.latitude(), 1), - 1 - ); - qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "hash:coordinate" << tileHash << coordinate; + const double lat = fromCoord.latitude(); + const double lon = fromCoord.longitude(); + const int steps = qCeil(toCoord.distanceTo(fromCoord) / TerrainTileCopernicus::kTileValueSpacingMeters); // TODO: get spacing from terrainQueryInterface + const double latDiff = toCoord.latitude() - lat; + const double lonDiff = toCoord.longitude() - lon; - TerrainTile* const tile = _getCachedTile(tileHash); - if (tile) { - const double elevation = tile->elevation(coordinate); - if (qIsNaN(elevation)) { - error = true; - qCWarning(TerrainTileManagerLog) << Q_FUNC_INFO << "Internal Error: missing elevation in tile cache"; - } else { - qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "returning elevation from tile cache" << elevation; - } - altitudes.push_back(elevation); - } else if (_state != TerrainQuery::State::Downloading) { - QGeoTileSpec spec; - spec.setX(provider->long2tileX(coordinate.longitude(), 1)); - spec.setY(provider->lat2tileY(coordinate.latitude(), 1)); - spec.setZoom(1); - spec.setMapId(provider->getMapId()); - const QNetworkRequest request = QGeoTileFetcherQGC::getNetworkRequest(spec.mapId(), spec.x(), spec.y(), spec.zoom()); - QGeoTiledMapReplyQGC* const reply = new QGeoTiledMapReplyQGC(_networkManager, request, spec, this); - (void) connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainTileManager::_terrainDone); - _state = TerrainQuery::State::Downloading; - // TODO: Batch Downloading? - return false; - } else { - return false; + QList coordinates; + if (steps == 0) { + (void) coordinates.append(fromCoord); + (void) coordinates.append(toCoord); + distanceBetween = finalDistanceBetween = coordinates[0].distanceTo(coordinates[1]); + } else { + for (int i = 0; i <= steps; i++) { + const double latStep = lat + ((latDiff * static_cast(i)) / static_cast(steps)); + const double lonStep = lon + ((lonDiff * static_cast(i)) / static_cast(steps)); + (void) coordinates.append(QGeoCoordinate(latStep, lonStep)); } + + // We always have one too many and we always want the last one to be the endpoint + coordinates.last() = toCoord; + distanceBetween = coordinates[0].distanceTo(coordinates[1]); + finalDistanceBetween = coordinates[coordinates.count() - 2].distanceTo(coordinates.last()); } - return true; + qCDebug(TerrainTileManagerLog) << Q_FUNC_INFO << "fromCoord:toCoord:distanceBetween:finalDisanceBetween:coordCount" << fromCoord << toCoord << distanceBetween << finalDistanceBetween << coordinates.count(); + + return coordinates; } void TerrainTileManager::_tileFailed() diff --git a/src/Terrain/TerrainTileManager.h b/src/Terrain/TerrainTileManager.h index b2354cd0f65..b6ba5dedaee 100644 --- a/src/Terrain/TerrainTileManager.h +++ b/src/Terrain/TerrainTileManager.h @@ -19,6 +19,7 @@ class TerrainTile; class QNetworkAccessManager; +class UnitTestTerrainQuery; Q_DECLARE_LOGGING_CATEGORY(TerrainTileManagerLog) @@ -26,27 +27,27 @@ class TerrainTileManager : public QObject { Q_OBJECT + friend class UnitTestTerrainQuery; public: explicit TerrainTileManager(QObject *parent = nullptr); ~TerrainTileManager(); static TerrainTileManager *instance(); - void addCoordinateQuery(TerrainQueryInterface *terrainQueryInterface, const QList &coordinates); - void addPathQuery(TerrainQueryInterface *terrainQueryInterface, const QGeoCoordinate &startPoint, const QGeoCoordinate &endPoint); - /// Either returns altitudes from cache or queues database request /// @param[out] error true: altitude not returned due to error, false: altitudes returned /// @return true: altitude returned (check error as well), false: database query queued (altitudes not returned) bool getAltitudesForCoordinates(const QList &coordinates, QList &altitudes, bool &error); - /// Returns a list of individual coordinates along the requested path spaced according to the terrain tile value spacing - static QList pathQueryToCoords(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord, double &distanceBetween, double &finalDistanceBetween); + void addCoordinateQuery(TerrainQueryInterface *terrainQueryInterface, const QList &coordinates); + void addPathQuery(TerrainQueryInterface *terrainQueryInterface, const QGeoCoordinate &startPoint, const QGeoCoordinate &endPoint); private slots: void _terrainDone(); private: + /// Returns a list of individual coordinates along the requested path spaced according to the terrain tile value spacing + static QList _pathQueryToCoords(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord, double &distanceBetween, double &finalDistanceBetween); void _tileFailed(); void _cacheTile(const QByteArray &data, const QString &hash); TerrainTile *_getCachedTile(const QString &hash); diff --git a/src/UI/preferences/MapSettings.qml b/src/UI/preferences/MapSettings.qml index 3f43cfba805..9c9ef5a8f16 100644 --- a/src/UI/preferences/MapSettings.qml +++ b/src/UI/preferences/MapSettings.qml @@ -34,6 +34,7 @@ Item { property Fact _mapProviderFact: _settingsManager.flightMapSettings.mapProvider property Fact _mapTypeFact: _settingsManager.flightMapSettings.mapType + property Fact _elevationProviderFact: _settingsManager.flightMapSettings.elevationMapProvider property Fact _mapboxFact: _settingsManager ? _settingsManager.appSettings.mapboxToken : null property Fact _mapboxAccountFact: _settingsManager ? _settingsManager.appSettings.mapboxAccount : null property Fact _mapboxStyleFact: _settingsManager ? _settingsManager.appSettings.mapboxStyle : null @@ -85,6 +86,19 @@ Item { comboBox.currentIndex = index } } + + LabelledComboBox { + label: qsTr("Elevation Provider") + model: _mapEngineManager.elevationProviderList + + onActivated: (index) => { _elevationProviderFact.rawValue = comboBox.textAt(index) } + + Component.onCompleted: { + var index = comboBox.find(_elevationProviderFact.rawValue) + if (index < 0) index = 0 + comboBox.currentIndex = index + } + } } SettingsGroupLayout { diff --git a/test/Terrain/TerrainQueryTest.cc b/test/Terrain/TerrainQueryTest.cc index 4c0b5d29536..ad1d1dca9ef 100644 --- a/test/Terrain/TerrainQueryTest.cc +++ b/test/Terrain/TerrainQueryTest.cc @@ -103,7 +103,7 @@ void UnitTestTerrainQuery::requestCarpetHeights(const QGeoCoordinate &swCoord, c UnitTestTerrainQuery::PathHeightInfo_t UnitTestTerrainQuery::_requestPathHeights(const QGeoCoordinate &fromCoord, const QGeoCoordinate &toCoord) { PathHeightInfo_t pathHeights; - pathHeights.rgCoords = TerrainTileManager::pathQueryToCoords(fromCoord, toCoord, pathHeights.distanceBetween, pathHeights.finalDistanceBetween); + pathHeights.rgCoords = TerrainTileManager::_pathQueryToCoords(fromCoord, toCoord, pathHeights.distanceBetween, pathHeights.finalDistanceBetween); pathHeights.rgHeights = _requestCoordinateHeights(pathHeights.rgCoords); return pathHeights; }