Skip to content

Commit

Permalink
QtLocationPlugin: Add Elevation Provider Selection
Browse files Browse the repository at this point in the history
  • Loading branch information
HTRamsey committed Nov 6, 2024
1 parent b8d721e commit 3dd11b3
Show file tree
Hide file tree
Showing 34 changed files with 1,183 additions and 496 deletions.
7 changes: 4 additions & 3 deletions src/QmlControls/QGroundControlQmlGlobal.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -336,12 +337,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
Expand Down
4 changes: 2 additions & 2 deletions src/QmlControls/QGroundControlQmlGlobal.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 ();
Expand Down
1 change: 1 addition & 0 deletions src/QtLocationPlugin/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ target_link_libraries(QGCLocation
PRIVATE
Qt6::Positioning
Qt6::Sql
Compression
QGC
Settings
Utilities
Expand Down
130 changes: 122 additions & 8 deletions src/QtLocationPlugin/Providers/ElevationMapProvider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,27 +14,32 @@

#include "ElevationMapProvider.h"
#include "TerrainTileCopernicus.h"
#include "TerrainTileArduPilot.h"
#include "QGCZip.h"

#include <QtCore/QDir>
#include <QtCore/QTemporaryFile>

int CopernicusElevationProvider::long2tileX(double lon, int z) const
{
Q_UNUSED(z)
return static_cast<int>(floor((lon + 180.0) / TerrainTileCopernicus::tileSizeDegrees));
return static_cast<int>(floor((lon + 180.0) / TerrainTileCopernicus::kTileSizeDegrees));
}

int CopernicusElevationProvider::lat2tileY(double lat, int z) const
{
Q_UNUSED(z)
return static_cast<int>(floor((lat + 90.0) / TerrainTileCopernicus::tileSizeDegrees));
return static_cast<int>(floor((lat + 90.0) / TerrainTileCopernicus::kTileSizeDegrees));
}

QString CopernicusElevationProvider::_getURL(int x, int y, int zoom) const
{
Q_UNUSED(zoom)
return _mapUrl
.arg((static_cast<double>(y) * TerrainTileCopernicus::tileSizeDegrees) - 90.0)
.arg((static_cast<double>(x) * TerrainTileCopernicus::tileSizeDegrees) - 180.0)
.arg((static_cast<double>(y + 1) * TerrainTileCopernicus::tileSizeDegrees) - 90.0)
.arg((static_cast<double>(x + 1) * TerrainTileCopernicus::tileSizeDegrees) - 180.0);
const int lat1 = (static_cast<double>(y) * TerrainTileCopernicus::kTileSizeDegrees) - 90.0;
const int lon1 = (static_cast<double>(x) * TerrainTileCopernicus::kTileSizeDegrees) - 180.0;
const int lat2 = (static_cast<double>(y + 1) * TerrainTileCopernicus::kTileSizeDegrees) - 90.0;
const int lon2 = (static_cast<double>(x + 1) * TerrainTileCopernicus::kTileSizeDegrees) - 180.0;
return _mapUrl.arg(lat1).arg(lon1).arg(lat2).arg(lon2);
}

QGCTileSet CopernicusElevationProvider::getTileCount(int zoom, double topleftLon,
Expand All @@ -59,5 +64,114 @@ 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<int>(floor((lon + 180.0) / TerrainTileArduPilot::kTileSizeDegrees));
}

int ArduPilotTerrainElevationProvider::lat2tileY(double lat, int z) const
{
Q_UNUSED(z)
return static_cast<int>(floor((lat + 90.0) / TerrainTileArduPilot::kTileSizeDegrees));
}

QString ArduPilotTerrainElevationProvider::_getURL(int x, int y, int zoom) const
{
Q_UNUSED(zoom)

const int xForUrl = (static_cast<double>(x) * TerrainTileArduPilot::kTileSizeDegrees) - 180.0;
const int yForUrl = (static_cast<double>(y) * TerrainTileArduPilot::kTileSizeDegrees) - 90.0;

if ((xForUrl < -180) || (xForUrl > 180) || (yForUrl < -180) || (yForUrl > 180)) {
qCWarning(MapProviderLog) << "Invalid x or y values for URL generation:" << x << y;
return QString();
}

QString formattedYLat;
if (yForUrl >= 0) {
formattedYLat = QString("N%1").arg(QString::number(yForUrl).rightJustified(2, '0'));
} else {
formattedYLat = QString("S%1").arg(QString::number(-yForUrl).rightJustified(2, '0'));
}

QString formattedXLong;
if (xForUrl >= 0) {
formattedXLong = QString("E%1").arg(QString::number(xForUrl).rightJustified(3, '0'));
} else {
formattedXLong = QString("W%1").arg(QString::number(-xForUrl).rightJustified(3, '0'));
}

const QString url = _mapUrl.arg(formattedYLat, formattedXLong);
return url;
}

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<quint64>(set.tileX1) -
static_cast<quint64>(set.tileX0) + 1) *
(static_cast<quint64>(set.tileY1) -
static_cast<quint64>(set.tileY0) + 1);

set.tileSize = getAverageSize() * set.tileCount;

return set;
}

QByteArray ArduPilotTerrainElevationProvider::serialize(const QByteArray &image) const
{
QTemporaryFile tempFile(QDir::tempPath() + "/XXXXXX.zip");
tempFile.setFileTemplate(QDir::tempPath() + "/XXXXXX.zip");

if (!tempFile.open()) {
qCDebug(MapProviderLog) << "Could not create temporary file for zip data.";
return QByteArray();
}

tempFile.write(image);
tempFile.close();

// QTemporaryDir
const QString outputDirectoryPath = QDir::tempPath() + "/QGC/Location/Elevation/SRTM1";
QDir outputDirectory(outputDirectoryPath);
if (!outputDirectory.exists()) {
outputDirectory.mkpath(outputDirectoryPath);
}

if (!QGCZip::unzipFile(tempFile.fileName(), outputDirectoryPath)) {
qCDebug(MapProviderLog) << "Unzipping failed!";
return QByteArray();
}

const QStringList files = outputDirectory.entryList(QDir::Files);
if (files.isEmpty()) {
qCDebug(MapProviderLog) << "No files found in the unzipped directory!";
return QByteArray();
}

const QString filename = files.constFirst();
const QString unzippedFilePath = outputDirectoryPath + "/" + filename;
QFile file(unzippedFilePath);
if (!file.open(QIODevice::ReadOnly)) {
qCDebug(MapProviderLog) << "Could not open unzipped file for reading:" << unzippedFilePath;
return QByteArray();
}

const QByteArray result = file.readAll();
file.close();

return TerrainTileArduPilot::serializeFromData(filename, result);
}
48 changes: 40 additions & 8 deletions src/QtLocationPlugin/Providers/ElevationMapProvider.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@

#include "MapProvider.h"

static constexpr const quint32 AVERAGE_COPERNICUS_ELEV_SIZE = 2786;

class ElevationProvider : public MapProvider
{
protected:
Expand All @@ -30,16 +28,17 @@ 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) {}
kAvgElevSize,
QGeoMapType::TerrainMap) {}

int long2tileX(double lon, int z) const final;
int lat2tileY(double lat, int z) const final;
Expand All @@ -50,11 +49,44 @@ class CopernicusElevationProvider : public ElevationProvider

QByteArray serialize(const QByteArray &image) const final;

static constexpr const char *kProviderKey = "Copernicus Elevation";
static constexpr const char *kProviderKey = "Copernicus";
static constexpr const char *kProviderNotice = "© Airbus Defence and Space GmbH";
static constexpr const char *kProviderURL = "https://terrain-ce.suite.auterion.com";
static constexpr quint32 kAvgElevSize = 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"),
kAvgElevSize,
QGeoMapType::TerrainMap) {}

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 SRTM1";
static constexpr const char *kProviderNotice = "© ArduPilot.org";
static constexpr const char *kProviderURL = "https://terrain.ardupilot.org/SRTM1";
static constexpr quint32 kAvgElevSize = 50000;

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");
};
3 changes: 1 addition & 2 deletions src/QtLocationPlugin/QGCCachedTileSet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
#include <QGCApplication.h>
#include <QGCFileDownload.h>
#include <QGCLoggingCategory.h>
#include <TerrainTile.h>

#include <QtNetwork/QNetworkProxy>

Expand Down Expand Up @@ -217,7 +216,7 @@ void QGCCachedTileSet::_networkReplyFinished()
return;
}

const QString type = UrlFactory::tileHashToType(hash);
const QString type = UrlFactory::tileHashToType(hash); // TODO: Type is null for elevation
const SharedMapProvider mapProvider = UrlFactory::getMapProviderFromProviderType(type);
Q_CHECK_PTR(mapProvider);

Expand Down
22 changes: 19 additions & 3 deletions src/QtLocationPlugin/QGCMapUrlEngine.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,7 @@ const QList<SharedMapProvider> UrlFactory::_providers = {
std::make_shared<VWorldSatMapProvider>(),

std::make_shared<CopernicusElevationProvider>(),
std::make_shared<ArduPilotTerrainElevationProvider>(),

std::make_shared<JapanStdMapProvider>(),
std::make_shared<JapanSeamlessMapProvider>(),
Expand Down Expand Up @@ -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;
Expand All @@ -242,18 +255,21 @@ QStringList UrlFactory::getProviderTypes()
QString UrlFactory::providerTypeFromHash(int hash)
{
for (const SharedMapProvider &provider : _providers) {
if (hashFromProviderType(provider->getMapName()) == hash) {
return provider->getMapName();
const QString mapName = provider->getMapName();
if (hashFromProviderType(mapName) == hash) {
return mapName;
}
}

qCWarning(QGCMapUrlEngineLog) << Q_FUNC_INFO << "provider not found from hash:" << hash;
return QStringLiteral("");
}

// This seems to limit provider name length to less than ~25 chars due to downcasting to int
int UrlFactory::hashFromProviderType(QStringView type)
{
return static_cast<int>(qHash(type) >> 1);
const auto hash = qHash(type) >> 1;
return static_cast<int>(hash);
}

QString UrlFactory::tileHashToType(QStringView tileHash)
Expand Down
1 change: 1 addition & 0 deletions src/QtLocationPlugin/QGCMapUrlEngine.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ class UrlFactory
QStringView mapType);

static const QList<std::shared_ptr<const MapProvider>>& getProviders() { return _providers; }
static QStringList getElevationProviderTypes();
static QStringList getProviderTypes();
static int getQtMapIdFromProviderType(QStringView type);
static QString getProviderTypeFromQtMapId(int qtMapId);
Expand Down
Loading

0 comments on commit 3dd11b3

Please sign in to comment.