From bfdc40f19a704e2c2f9b8774ae0d9b6ec8b092ed Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 15 Nov 2017 15:07:04 -0500 Subject: [PATCH 01/29] Terrain tiles --- qgroundcontrol.pro | 4 ++ src/Terrain.h | 9 +++ src/TerrainCacheTileServer.cc | 6 ++ src/TerrainCacheTileServer.h | 19 +++++ src/TerrainTile.cc | 156 ++++++++++++++++++++++++++++++++++++++++++ src/TerrainTile.h | 95 +++++++++++++++++++++++++ 6 files changed, 289 insertions(+) create mode 100644 src/TerrainCacheTileServer.cc create mode 100644 src/TerrainCacheTileServer.h create mode 100644 src/TerrainTile.cc create mode 100644 src/TerrainTile.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index df35d13..bcc159a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -569,6 +569,8 @@ HEADERS += \ src/Settings/UnitsSettings.h \ src/Settings/VideoSettings.h \ src/Terrain.h \ + src/TerrainCacheTileServer.h \ + src/TerrainTile.h \ src/Vehicle/MAVLinkLogManager.h \ src/VehicleSetup/JoystickConfigController.h \ src/comm/LinkConfiguration.h \ @@ -756,6 +758,8 @@ SOURCES += \ src/Settings/UnitsSettings.cc \ src/Settings/VideoSettings.cc \ src/Terrain.cc \ + src/TerrainCacheTileServer.cc \ + src/TerrainTile.cc\ src/Vehicle/MAVLinkLogManager.cc \ src/VehicleSetup/JoystickConfigController.cc \ src/comm/LinkConfiguration.cc \ diff --git a/src/Terrain.h b/src/Terrain.h index bfc987f..9b3217e 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -13,6 +13,8 @@ #include #include +#include "TerrainCacheTileServer.h" + /* usage example: ElevationProvider *p = new ElevationProvider(); QList coordinates; @@ -38,6 +40,13 @@ public: */ bool queryTerrainData(const QList& coordinates); + /** + * + * + * + */ + bool cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); + signals: void terrainData(bool success, QList altitudes); diff --git a/src/TerrainCacheTileServer.cc b/src/TerrainCacheTileServer.cc new file mode 100644 index 0000000..1dde372 --- /dev/null +++ b/src/TerrainCacheTileServer.cc @@ -0,0 +1,6 @@ +#include "TerrainCacheTileServer.h" + +TerrainCacheTileServer::TerrainCacheTileServer() +{ + +} diff --git a/src/TerrainCacheTileServer.h b/src/TerrainCacheTileServer.h new file mode 100644 index 0000000..6edc14d --- /dev/null +++ b/src/TerrainCacheTileServer.h @@ -0,0 +1,19 @@ +#ifndef TERRAINCACHESERVER_H +#define TERRAINCACHESERVER_H + +#include "TerrainTile.h" + +class TerrainCacheTileServer +{ +public: + TerrainCacheTileServer(); + + bool cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); + + bool cached(const QGeoCoordinate& coord); + +private: + QStringList _downloadQueue; +}; + +#endif // TERRAINCACHESERVER_H diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc new file mode 100644 index 0000000..e919137 --- /dev/null +++ b/src/TerrainTile.cc @@ -0,0 +1,156 @@ +#include "TerrainTile.h" +#include "JsonHelper.h" + +#include +#include +#include + +QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") + +const char* TerrainTile::_jsonStatusKey = "status"; +const char* TerrainTile::_jsonDataKey = "data"; +const char* TerrainTile::_jsonBoundsKey = "bounds"; +const char* TerrainTile::_jsonSouthWestKey = "sw"; +const char* TerrainTile::_jsonNorthEastKey = "ne"; +const char* TerrainTile::_jsonStatsKey = "stats"; +const char* TerrainTile::_jsonMaxElevationKey = "max"; +const char* TerrainTile::_jsonMinElevationKey = "min"; +const char* TerrainTile::_jsonAvgElevationKey = "avg"; +const char* TerrainTile::_jsonCarpetKey = "carpet"; + +TerrainTile::TerrainTile() + : _minElevation(-1.0) + , _maxElevation(-1.0) + , _avgElevation(-1.0) + , _isValid(false) +{ + +} + +TerrainTile::~TerrainTile() +{ +} + +TerrainTile::TerrainTile(QJsonDocument doc) + : _minElevation(-1.0) + , _maxElevation(-1.0) + , _avgElevation(-1.0) + , _isValid(false) +{ + if (!doc.isObject()) { + qCDebug(TerrainTileLog) << "Terrain tile json doc is no object"; + return; + } + QJsonObject rootObject = doc.object(); + + QString errorString; + QList rootVersionKeyInfoList = { + { _jsonStatusKey, QJsonValue::String, true }, + { _jsonDataKey, QJsonValue::String, true }, + }; + if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + return false; + } + + if (rootObject[_jsonStatusKey].toString() != "success") { + qCDebug(TerrainTileLog) << "Invalid terrain tile."; + return; + } + const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject(); + QList dataVersionKeyInfoList = { + { _jsonBoundsKey, QJsonValue::Object, true }, + { _jsonStatsKey, QJsonValue::Object, true }, + { _jsonCarpetKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + return false; + } + + // Bounds + const QJsonObject& boundsObject = dataObject[_jsonBoundsKey].toObject(); + QList boundsVersionKeyInfoList = { + { _jsonSouthWestKey, QJsonValue::Array, true }, + { _jsonNorthEastKey, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + return false; + } + const QJsonArray& swArray = boundsObject[_jsonSouthWestKey].toArray(); + const QJsonArray& neArray = boundsObject[_jsonNorthEastKey].toArray(); + if (swArray.count() < 2 || neArray.count() < 2 ) { + qCDebug(TerrainTileLog) << "Incomplete bounding location"; + return; + } + _southWest.setLatitude(swArray[0].toDouble()); + _southWest.setLongitude(swArray[1].toDouble()); + _northEast.setLatitude(neArray[0].toDouble()); + _northEast.setLongitude(neArray[1].toDouble()); + + // Stats + const QJsonObject& statsObject = dataObject[_jsonBoundsKey].toObject(); + QList statsVersionKeyInfoList = { + { _jsonMaxElevationKey, QJsonValue::Double, true }, + { _jsonMinElevationKey, QJsonValue::Double, true }, + { _jsonAvgElevationKey, QJsonValue::Double, true }, + }; + if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) { + qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; + return false; + } + _maxElevation = statsObject[_jsonMaxElevationKey].toInt(); + _minElevation = statsObject[_jsonMinElevationKey].toInt(); + _avgElevation = statsObject[_jsonAvgElevationKey].toInt(); + + // Carpet + const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); + if (carpetArray.count() != _gridSize) { + qCDebug(TerrainTileLog) << "Expected array of " << _gridSize << ", instead got " << carpetArray.count(); + return; + } + for (int i = 0; i < _gridSize; i++) { + const QJsonArray& row = carpetArray[i].toArray(); + if (row.count() != _gridSize) { + qCDebug(TerrainTileLog) << "Expected row array of " << _gridSize << ", instead got " << row.count(); + return; + } + for (int j = 0; j < _gridSize; j++) { + _data[i][j] = row[j].toDouble(); + } + } + _isValid = true; +} + +bool TerrainTile::isIn(QGeoCoordinate coord) +{ + if (!_isValid) { + qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; + return false; + } + bool ret = coord.latitude() >= _southWest.longitude() && coord.longitude() >= _southWest.longitude() && + coord.latitude() <= _northEast.longitude() && coord.longitude() <= _northEast.longitude(); + qCDebug(TerrainTileLog) << "Checking isIn: " << coord << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; + return ret; +} + +float TerrainTile::elevation(const QGeoCoordinate& coord) +{ + if (_isValid) { + qCDebug << "elevation: " << coord << " , in sw " << _southWest << " , ne " << _northEast; + // Get the index at resolution of 1 arc second + int indexLat = std::round((coord.latitude() - _southWest.latitude()) / _srtm1Increment); + int indexLon = std::round((coord.longitude() - _southWest.longitude()) / _srtm1Increment); + qCDebug << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. + Q_ASSERT(indexLat >= 0); + Q_ASSERT(indexLat < _gridSize); + Q_ASSERT(indexLon >= 0); + Q_ASSERT(indexLon < _gridSize); + qCDebug << "elevation" << _data[indexLat][indexLon]; + return _data[indexLat][indexLon]; + } else { + qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; + return -1.0; + } +} diff --git a/src/TerrainTile.h b/src/TerrainTile.h new file mode 100644 index 0000000..69e9db2 --- /dev/null +++ b/src/TerrainTile.h @@ -0,0 +1,95 @@ +#ifndef TERRAINTILE_H +#define TERRAINTILE_H + +#include "QGCLoggingCategory.h" + +#include + +#define TERRAIN_TILE_SIZE 90 + +Q_DECLARE_LOGGING_CATEGORY(TerrainTileLog) + +class TerrainTile +{ +public: + TerrainTile(); + ~TerrainTile(); + + /** + * Constructor from json doc with elevation data (either from file or web) + * + * @param json doc + */ + TerrainTile(QJsonDocument doc); + + /** + * Check for whether a coordinate lies within this tile + * + * @param coordinate + * @return true if within + */ + bool isIn(QGeoCoordinate coord); + + /** + * Check whether valid data is loaded + * + * @return true if data is valid + */ + bool isValid(void) { return _isValid; } + + /** + * Evaluates the elevation at the given coordinate + * + * @param coordinate + * @return elevation + */ + float elevation(const QGeoCoordinate& coord); + + /** + * Accessor for the minimum elevation of the tile + * + * @return minimum elevation + */ + float minElevation(void) { return _minElevation; } + + /** + * Accessor for the maximum elevation of the tile + * + * @return maximum elevation + */ + float maxElevation(void) { return _maxElevation; } + + /** + * Accessor for the average elevation of the tile + * + * @return average elevation + */ + float avgElevation(void) { return _avgElevation; } + +private: + QGeoCoordinate _southWest; /// South west corner of the tile + QGeoCoordinate _northEast; /// North east corner of the tile + + float _minElevation; /// Minimum elevation in tile + float _maxElevation; /// Maximum elevation in tile + float _avgElevation; /// Average elevation of the tile + + float _data[TERRAIN_TILE_SIZE][TERRAIN_TILE_SIZE]; /// elevation data + bool _isValid; /// data loaded is valid + static const int _gridSize = TERRAIN_TILE_SIZE; /// tile grid size in lat and lon + static const float _srtm1Increment = 1.0 / (60.0 * 60.0); /// grid spacing in degree + + // Json keys + static const char* _jsonStatusKey; + static const char* _jsonDataKey; + static const char* _jsonBoundsKey; + static const char* _jsonSouthWestKey; + static const char* _jsonNorthEastKey; + static const char* _jsonStatsKey; + static const char* _jsonMaxElevationKey; + static const char* _jsonMinElevationKey; + static const char* _jsonAvgElevationKey; + static const char* _jsonCarpetKey; +}; + +#endif // TERRAINTILE_H From 54c7e7ddaf0c2dc403c5764452c6b46c83842d6a Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 15 Nov 2017 17:03:05 -0500 Subject: [PATCH 02/29] api calls --- qgroundcontrol.pro | 2 - src/Terrain.cc | 141 +++++++++++++++++++++++++++++++++++++++++- src/Terrain.h | 28 ++++++--- src/TerrainCacheTileServer.cc | 6 -- src/TerrainCacheTileServer.h | 19 ------ src/TerrainTile.cc | 10 +-- src/TerrainTile.h | 24 ++++--- 7 files changed, 179 insertions(+), 51 deletions(-) delete mode 100644 src/TerrainCacheTileServer.cc delete mode 100644 src/TerrainCacheTileServer.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bcc159a..4184722 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -569,7 +569,6 @@ HEADERS += \ src/Settings/UnitsSettings.h \ src/Settings/VideoSettings.h \ src/Terrain.h \ - src/TerrainCacheTileServer.h \ src/TerrainTile.h \ src/Vehicle/MAVLinkLogManager.h \ src/VehicleSetup/JoystickConfigController.h \ @@ -758,7 +757,6 @@ SOURCES += \ src/Settings/UnitsSettings.cc \ src/Settings/VideoSettings.cc \ src/Terrain.cc \ - src/TerrainCacheTileServer.cc \ src/TerrainTile.cc\ src/Vehicle/MAVLinkLogManager.cc \ src/VehicleSetup/JoystickConfigController.cc \ diff --git a/src/Terrain.cc b/src/Terrain.cc index 9203481..2b7b896 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -18,6 +18,8 @@ #include #include +QGC_LOGGING_CATEGORY(TerrainLog, "TerrainLog") + ElevationProvider::ElevationProvider(QObject* parent) : QObject(parent) { @@ -26,7 +28,37 @@ ElevationProvider::ElevationProvider(QObject* parent) bool ElevationProvider::queryTerrainData(const QList& coordinates) { - if (_state != State::Idle || coordinates.length() == 0) { + if (coordinates.length() == 0) { + return false; + } + + bool fallBackToOnline = false; + QList altitudes; + foreach (QGeoCoordinate coordinate, coordinates) { + QString uniqueTileId = _uniqueTileId(coordinate); + _tilesMutex.lock(); + if (!_tiles.contains(uniqueTileId)) { + _tilesMutex.unlock(); + fallBackToOnline = true; + break; + } else { + if (_tiles[uniqueTileId].isIn(coordinate)) { + altitudes.push_back(_tiles[uniqueTileId].elevation(coordinate)); + } else { + qCDebug(TerrainLog) << "Error: coordinate not in tile region"; + altitudes.push_back(-1.0); + } + } + _tilesMutex.unlock(); + } + + if (!fallBackToOnline) { + qCDebug(TerrainLog) << "All altitudes taken from cached data"; + emit terrainData(true, altitudes); + return true; + } + + if (_state != State::Idle) { return false; } @@ -59,6 +91,27 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate return true; } +bool ElevationProvider::cacheTerrainTiles(const QList& coordinates) +{ + if (coordinates.length() == 0) { + return false; + } + + for (const auto& coordinate : coordinates) { + QString uniqueTileId = _uniqueTileId(coordinate); + _tilesMutex.lock(); + if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { + continue + } + _downloadQueue.append(uniqueTileId.replace("-", ",")); + _tilesMutex.unlock(); + qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; + } + + _downloadTiles(); + return true; +} + void ElevationProvider::_requestFinished() { QNetworkReply* reply = qobject_cast(QObject::sender()); @@ -71,7 +124,7 @@ void ElevationProvider::_requestFinished() QJsonParseError parseError; QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - qDebug() << responseJson; + qCDebug(TerrainLog) << "Received " << responseJson; emit terrainData(false, altitudes); return; } @@ -96,3 +149,87 @@ void ElevationProvider::_requestFinished() } emit terrainData(false, altitudes); } + +void ElevationProvider::_requestFinishedTile() +{ + QNetworkReply* reply = qobject_cast(QObject::sender()); + _state = State::Idle; + + // When an error occurs we still end up here + if (reply->error() != QNetworkReply::NoError) { + QByteArray responseBytes = reply->readAll(); + + QJsonParseError parseError; + QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); + qCDebug(TerrainLog) << "ERROR: Received " << responseJson; + // TODO: Handle error in downloading data + return; + } + + QByteArray responseBytes = reply->readAll(); + + QJsonParseError parseError; + QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); + if (parseError.error != QJsonParseError::NoError) { + // TODO: Handle error in downloading data + return; + } + + TerrainTile* tile = new TerrainTile(responseJson); + if (tile->isValid()) { + _tilesMutex.lock(); + if (!_tiles.contains(_uniqueTileId(tile->centerCoordinate()))) { + _tiles.insert(_uniqueTileId(tile->centerCoordinate()), *tile); + } else { + delete tile; + } + _tilesMutex.lock(); + } + + _downloadTiles(); +} + +void ElevationProvider::_downloadTiles(void) +{ + if (_state == State::Idle && _downloadQueue.count() > 0) { + QUrlQuery query; + _tilesMutex.lock(); + qCDebug(TerrainLog) << "Starting download for " << _downloadQueue.first(); + query.addQueryItem(QStringLiteral("points"), _downloadQueue.first()); + _downloadQueue.pop_front(); + _tilesMutex.unlock(); + QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/carpet")); + url.setQuery(query); + + QNetworkRequest request(url); + + QNetworkProxy tProxy; + tProxy.setType(QNetworkProxy::DefaultProxy); + _networkManager.setProxy(tProxy); + + QNetworkReply* networkReply = _networkManager.get(request); + if (!networkReply) { + return false; + } + + connect(networkReply, &QNetworkReply::finished, this, &ElevationProvider::_requestFinishedTile); + + _state = State::Downloading; + } +} + +QString ElevationProvider::_uniqueTileId(const QGeoCoordinate& coordinate) +{ + QGeoCoordinate southEast; + southEast.setLatitude(floor(coordinate.latitude()*40.0)/40.0); + southEast.setLongitude(floor(coordinate.longitude()*40.0)/40.0); + QGeoCoordinate northEast; + northEast.setLatitude(ceil(coordinate.latitude()*40.0)/40.0); + northEast.setLongitude(ceil(coordinate.longitude()*40.0)/40.0); + + QString ret = QString::number(southEast.latitude(), 'f', 6) + "-" + QString::number(southEast.longitude(), 'f', 6) + "-" + + QString::number(northEast.latitude(), 'f', 6) + "-" + QString::number(northEast.longitude(), 'f', 6); + qCDebug << "Computing unique tile id for " << coordinate << ret; + + return ret; +} diff --git a/src/Terrain.h b/src/Terrain.h index 9b3217e..9422f4d 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -9,11 +9,14 @@ #pragma once +#include "TerrainTile.h" +#include "QGCLoggingCategory.h" + #include #include #include - -#include "TerrainCacheTileServer.h" +#include +#include /* usage example: ElevationProvider *p = new ElevationProvider(); @@ -25,6 +28,7 @@ p->queryTerrainData(coordinates); */ +Q_DECLARE_LOGGING_CATEGORY(TerrainLog) class ElevationProvider : public QObject { @@ -41,24 +45,34 @@ public: bool queryTerrainData(const QList& coordinates); /** + * Cache all data in rectangular region given by south west and north east corner. * - * - * + * @param southWest + * @param northEast */ - bool cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); + void cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); signals: void terrainData(bool success, QList altitudes); private slots: void _requestFinished(); + void _requestFinishedTile(); + private: + QString _uniqueTileId(const QGeoCoordinate& coordinate); + void _downloadTiles(void); + enum class State { Idle, Downloading, }; - State _state = State::Idle; - QNetworkAccessManager _networkManager; + State _state = State::Idle; + QNetworkAccessManager _networkManager; + + static QMutex _tilesMutex; + static QHash _tiles; + QStringList _downloadQueue; }; diff --git a/src/TerrainCacheTileServer.cc b/src/TerrainCacheTileServer.cc deleted file mode 100644 index 1dde372..0000000 --- a/src/TerrainCacheTileServer.cc +++ /dev/null @@ -1,6 +0,0 @@ -#include "TerrainCacheTileServer.h" - -TerrainCacheTileServer::TerrainCacheTileServer() -{ - -} diff --git a/src/TerrainCacheTileServer.h b/src/TerrainCacheTileServer.h deleted file mode 100644 index 6edc14d..0000000 --- a/src/TerrainCacheTileServer.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef TERRAINCACHESERVER_H -#define TERRAINCACHESERVER_H - -#include "TerrainTile.h" - -class TerrainCacheTileServer -{ -public: - TerrainCacheTileServer(); - - bool cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); - - bool cached(const QGeoCoordinate& coord); - -private: - QStringList _downloadQueue; -}; - -#endif // TERRAINCACHESERVER_H diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index e919137..c9da591 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -31,17 +31,17 @@ TerrainTile::~TerrainTile() { } -TerrainTile::TerrainTile(QJsonDocument doc) +TerrainTile::TerrainTile(QJsonDocument document) : _minElevation(-1.0) , _maxElevation(-1.0) , _avgElevation(-1.0) , _isValid(false) { - if (!doc.isObject()) { + if (!document.isObject()) { qCDebug(TerrainTileLog) << "Terrain tile json doc is no object"; return; } - QJsonObject rootObject = doc.object(); + QJsonObject rootObject = document.object(); QString errorString; QList rootVersionKeyInfoList = { @@ -123,7 +123,7 @@ TerrainTile::TerrainTile(QJsonDocument doc) _isValid = true; } -bool TerrainTile::isIn(QGeoCoordinate coord) +bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const { if (!_isValid) { qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; @@ -135,7 +135,7 @@ bool TerrainTile::isIn(QGeoCoordinate coord) return ret; } -float TerrainTile::elevation(const QGeoCoordinate& coord) +float TerrainTile::elevation(const QGeoCoordinate& coordinate) const { if (_isValid) { qCDebug << "elevation: " << coord << " , in sw " << _southWest << " , ne " << _northEast; diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 69e9db2..bd12697 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -18,9 +18,9 @@ public: /** * Constructor from json doc with elevation data (either from file or web) * - * @param json doc + * @param document */ - TerrainTile(QJsonDocument doc); + TerrainTile(QJsonDocument document); /** * Check for whether a coordinate lies within this tile @@ -28,14 +28,14 @@ public: * @param coordinate * @return true if within */ - bool isIn(QGeoCoordinate coord); + bool isIn(const QGeoCoordinate& coordinate) const; /** * Check whether valid data is loaded * * @return true if data is valid */ - bool isValid(void) { return _isValid; } + bool isValid(void) const { return _isValid; } /** * Evaluates the elevation at the given coordinate @@ -43,28 +43,34 @@ public: * @param coordinate * @return elevation */ - float elevation(const QGeoCoordinate& coord); + float elevation(const QGeoCoordinate& coordinate) const; /** * Accessor for the minimum elevation of the tile * * @return minimum elevation */ - float minElevation(void) { return _minElevation; } + float minElevation(void) const { return _minElevation; } /** * Accessor for the maximum elevation of the tile * * @return maximum elevation */ - float maxElevation(void) { return _maxElevation; } + float maxElevation(void) const { return _maxElevation; } /** * Accessor for the average elevation of the tile * * @return average elevation */ - float avgElevation(void) { return _avgElevation; } + float avgElevation(void) const { return _avgElevation; } + + /// tile grid size in lat and lon + static const int _gridSize = TERRAIN_TILE_SIZE; + + /// grid spacing in degree + static const float _srtm1Increment = 1.0 / (60.0 * 60.0); private: QGeoCoordinate _southWest; /// South west corner of the tile @@ -76,8 +82,6 @@ private: float _data[TERRAIN_TILE_SIZE][TERRAIN_TILE_SIZE]; /// elevation data bool _isValid; /// data loaded is valid - static const int _gridSize = TERRAIN_TILE_SIZE; /// tile grid size in lat and lon - static const float _srtm1Increment = 1.0 / (60.0 * 60.0); /// grid spacing in degree // Json keys static const char* _jsonStatusKey; From 17fe7893e90a8023549e2dacb41129df10600b92 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 15 Nov 2017 17:22:23 -0500 Subject: [PATCH 03/29] fixes --- src/Terrain.cc | 10 +++++++--- src/Terrain.h | 4 ++-- src/TerrainTile.cc | 30 ++++++++++++++++++------------ src/TerrainTile.h | 11 +++++++++-- 4 files changed, 36 insertions(+), 19 deletions(-) diff --git a/src/Terrain.cc b/src/Terrain.cc index 2b7b896..88fe899 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -20,6 +20,10 @@ QGC_LOGGING_CATEGORY(TerrainLog, "TerrainLog") +QMutex ElevationProvider::_tilesMutex; +QHash ElevationProvider::_tiles; +QStringList ElevationProvider::_downloadQueue; + ElevationProvider::ElevationProvider(QObject* parent) : QObject(parent) { @@ -101,7 +105,7 @@ bool ElevationProvider::cacheTerrainTiles(const QList& coordinat QString uniqueTileId = _uniqueTileId(coordinate); _tilesMutex.lock(); if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { - continue + continue; } _downloadQueue.append(uniqueTileId.replace("-", ",")); _tilesMutex.unlock(); @@ -209,7 +213,7 @@ void ElevationProvider::_downloadTiles(void) QNetworkReply* networkReply = _networkManager.get(request); if (!networkReply) { - return false; + return; } connect(networkReply, &QNetworkReply::finished, this, &ElevationProvider::_requestFinishedTile); @@ -229,7 +233,7 @@ QString ElevationProvider::_uniqueTileId(const QGeoCoordinate& coordinate) QString ret = QString::number(southEast.latitude(), 'f', 6) + "-" + QString::number(southEast.longitude(), 'f', 6) + "-" + QString::number(northEast.latitude(), 'f', 6) + "-" + QString::number(northEast.longitude(), 'f', 6); - qCDebug << "Computing unique tile id for " << coordinate << ret; + qCDebug(TerrainLog) << "Computing unique tile id for " << coordinate << ret; return ret; } diff --git a/src/Terrain.h b/src/Terrain.h index 9422f4d..5cfe3ce 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -50,7 +50,7 @@ public: * @param southWest * @param northEast */ - void cacheTerrainData(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); + bool cacheTerrainTiles(const QList& coordinates); signals: void terrainData(bool success, QList altitudes); @@ -74,5 +74,5 @@ private: static QMutex _tilesMutex; static QHash _tiles; - QStringList _downloadQueue; + static QStringList _downloadQueue; }; diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index c9da591..3af537e 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -7,6 +7,7 @@ QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") +const double TerrainTile::_srtm1TileSize = 0.025; const char* TerrainTile::_jsonStatusKey = "status"; const char* TerrainTile::_jsonDataKey = "data"; const char* TerrainTile::_jsonBoundsKey = "bounds"; @@ -50,7 +51,7 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return false; + return; } if (rootObject[_jsonStatusKey].toString() != "success") { @@ -65,7 +66,7 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return false; + return; } // Bounds @@ -76,7 +77,7 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return false; + return; } const QJsonArray& swArray = boundsObject[_jsonSouthWestKey].toArray(); const QJsonArray& neArray = boundsObject[_jsonNorthEastKey].toArray(); @@ -98,7 +99,7 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return false; + return; } _maxElevation = statsObject[_jsonMaxElevationKey].toInt(); _minElevation = statsObject[_jsonMinElevationKey].toInt(); @@ -129,28 +130,33 @@ bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; return false; } - bool ret = coord.latitude() >= _southWest.longitude() && coord.longitude() >= _southWest.longitude() && - coord.latitude() <= _northEast.longitude() && coord.longitude() <= _northEast.longitude(); - qCDebug(TerrainTileLog) << "Checking isIn: " << coord << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; + bool ret = coordinate.latitude() >= _southWest.longitude() && coordinate.longitude() >= _southWest.longitude() && + coordinate.latitude() <= _northEast.longitude() && coordinate.longitude() <= _northEast.longitude(); + qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; return ret; } float TerrainTile::elevation(const QGeoCoordinate& coordinate) const { if (_isValid) { - qCDebug << "elevation: " << coord << " , in sw " << _southWest << " , ne " << _northEast; + qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = std::round((coord.latitude() - _southWest.latitude()) / _srtm1Increment); - int indexLon = std::round((coord.longitude() - _southWest.longitude()) / _srtm1Increment); - qCDebug << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. + int indexLat = std::round((coordinate.latitude() - _southWest.latitude()) * _gridSize / _srtm1TileSize); + int indexLon = std::round((coordinate.longitude() - _southWest.longitude()) * _gridSize / _srtm1TileSize); + qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. Q_ASSERT(indexLat >= 0); Q_ASSERT(indexLat < _gridSize); Q_ASSERT(indexLon >= 0); Q_ASSERT(indexLon < _gridSize); - qCDebug << "elevation" << _data[indexLat][indexLon]; + qCDebug(TerrainTileLog) << "elevation" << _data[indexLat][indexLon]; return _data[indexLat][indexLon]; } else { qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; return -1.0; } } + +QGeoCoordinate TerrainTile::centerCoordinate(void) const +{ + return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); +} diff --git a/src/TerrainTile.h b/src/TerrainTile.h index bd12697..1c66cbe 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -66,11 +66,18 @@ public: */ float avgElevation(void) const { return _avgElevation; } + /** + * Accessor for the center coordinate + * + * @return center coordinate + */ + QGeoCoordinate centerCoordinate(void) const; + /// tile grid size in lat and lon static const int _gridSize = TERRAIN_TILE_SIZE; - /// grid spacing in degree - static const float _srtm1Increment = 1.0 / (60.0 * 60.0); + /// size of a tile in degree + static const double _srtm1TileSize; private: QGeoCoordinate _southWest; /// South west corner of the tile From 8b7553b582b7fa64b2e3725b33725e2af42a77ea Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 16 Nov 2017 10:55:34 -0500 Subject: [PATCH 04/29] download tiles when downloading offline map --- .../QMLControl/QGCMapEngineManager.cc | 2 + .../QMLControl/QGCMapEngineManager.h | 3 ++ src/Terrain.cc | 60 +++++++++++++++++----- src/Terrain.h | 11 +++- src/TerrainTile.cc | 30 +++++------ src/TerrainTile.h | 4 +- 6 files changed, 80 insertions(+), 30 deletions(-) diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index 52037ae..fce1e12 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -157,6 +157,8 @@ QGCMapEngineManager::startDownload(const QString& name, const QString& mapType) } else { qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; } + // TODO: this could also get some feedback + _elevationProvider.cacheTerrainTiles(QGeoCoordinate(_bottomRightLat, _topleftLon), QGeoCoordinate(_topleftLat, _bottomRightLon)); } //----------------------------------------------------------------------------- diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h index 0d1b419..73c9b73 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h @@ -19,6 +19,7 @@ #include "QGCLoggingCategory.h" #include "QGCMapEngine.h" #include "QGCMapTileSet.h" +#include "Terrain.h" Q_DECLARE_LOGGING_CATEGORY(QGCMapEngineManagerLog) @@ -152,6 +153,8 @@ private: int _actionProgress; ImportAction _importAction; bool _importReplace; + + ElevationProvider _elevationProvider; }; #endif diff --git a/src/Terrain.cc b/src/Terrain.cc index 88fe899..875a5d6 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -95,6 +95,37 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate return true; } +bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast) +{ + qCDebug(TerrainLog) << "Cache terrain tiles southWest:northEast" << southWest << northEast; + + // Correct corners of the tile to fixed grid + QGeoCoordinate southWestCorrected; + southWestCorrected.setLatitude(floor(southWest.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + southWestCorrected.setLongitude(floor(southWest.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + QGeoCoordinate northEastCorrected; + northEastCorrected.setLatitude(ceil(northEast.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + northEastCorrected.setLongitude(ceil(northEast.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + + // Add all tiles to download queue + for (double lat = southWestCorrected.latitude() + TerrainTile::srtm1TileSize / 2.0; lat < northEastCorrected.latitude(); lat += TerrainTile::srtm1TileSize) { + for (double lon = southWestCorrected.longitude() + TerrainTile::srtm1TileSize / 2.0; lon < northEastCorrected.longitude(); lon += TerrainTile::srtm1TileSize) { + QString uniqueTileId = _uniqueTileId(QGeoCoordinate(lat, lon)); + _tilesMutex.lock(); + if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { + _tilesMutex.unlock(); + continue; + } + _downloadQueue.append(uniqueTileId.replace("_", ",")); + _tilesMutex.unlock(); + qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; + } + } + + _downloadTiles(); + return true; +} + bool ElevationProvider::cacheTerrainTiles(const QList& coordinates) { if (coordinates.length() == 0) { @@ -105,9 +136,10 @@ bool ElevationProvider::cacheTerrainTiles(const QList& coordinat QString uniqueTileId = _uniqueTileId(coordinate); _tilesMutex.lock(); if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { + _tilesMutex.unlock(); continue; } - _downloadQueue.append(uniqueTileId.replace("-", ",")); + _downloadQueue.append(uniqueTileId.replace("_", ",")); _tilesMutex.unlock(); qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; } @@ -166,7 +198,8 @@ void ElevationProvider::_requestFinishedTile() QJsonParseError parseError; QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); qCDebug(TerrainLog) << "ERROR: Received " << responseJson; - // TODO: Handle error in downloading data + // TODO (birchera): Handle error in downloading data + _downloadTiles(); return; } @@ -175,7 +208,8 @@ void ElevationProvider::_requestFinishedTile() QJsonParseError parseError; QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); if (parseError.error != QJsonParseError::NoError) { - // TODO: Handle error in downloading data + // TODO (birchera): Handle error in downloading data + _downloadTiles(); return; } @@ -187,7 +221,7 @@ void ElevationProvider::_requestFinishedTile() } else { delete tile; } - _tilesMutex.lock(); + _tilesMutex.unlock(); } _downloadTiles(); @@ -202,7 +236,7 @@ void ElevationProvider::_downloadTiles(void) query.addQueryItem(QStringLiteral("points"), _downloadQueue.first()); _downloadQueue.pop_front(); _tilesMutex.unlock(); - QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/carpet")); + QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/ele/carpet")); url.setQuery(query); QNetworkRequest request(url); @@ -224,15 +258,17 @@ void ElevationProvider::_downloadTiles(void) QString ElevationProvider::_uniqueTileId(const QGeoCoordinate& coordinate) { - QGeoCoordinate southEast; - southEast.setLatitude(floor(coordinate.latitude()*40.0)/40.0); - southEast.setLongitude(floor(coordinate.longitude()*40.0)/40.0); + // Compute corners of the tile + QGeoCoordinate southWest; + southWest.setLatitude(floor(coordinate.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + southWest.setLongitude(floor(coordinate.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); QGeoCoordinate northEast; - northEast.setLatitude(ceil(coordinate.latitude()*40.0)/40.0); - northEast.setLongitude(ceil(coordinate.longitude()*40.0)/40.0); + northEast.setLatitude(ceil(coordinate.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + northEast.setLongitude(ceil(coordinate.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); - QString ret = QString::number(southEast.latitude(), 'f', 6) + "-" + QString::number(southEast.longitude(), 'f', 6) + "-" + - QString::number(northEast.latitude(), 'f', 6) + "-" + QString::number(northEast.longitude(), 'f', 6); + // Generate uniquely identifying string + QString ret = QString::number(southWest.latitude(), 'f', 6) + "_" + QString::number(southWest.longitude(), 'f', 6) + "_" + + QString::number(northEast.latitude(), 'f', 6) + "_" + QString::number(northEast.longitude(), 'f', 6); qCDebug(TerrainLog) << "Computing unique tile id for " << coordinate << ret; return ret; diff --git a/src/Terrain.h b/src/Terrain.h index 5cfe3ce..14cb99f 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -45,12 +45,21 @@ public: bool queryTerrainData(const QList& coordinates); /** + * Cache all data in rectangular region given by list of coordinates. + * + * @param coordinates + * @return true on successful scheduling for download + */ + bool cacheTerrainTiles(const QList& coordinates); + + /** * Cache all data in rectangular region given by south west and north east corner. * * @param southWest * @param northEast + * @return true on successful scheduling for download */ - bool cacheTerrainTiles(const QList& coordinates); + bool cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); signals: void terrainData(bool success, QList altitudes); diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 3af537e..8baca5f 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -7,7 +7,7 @@ QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") -const double TerrainTile::_srtm1TileSize = 0.025; +const double TerrainTile::srtm1TileSize = 0.025; const char* TerrainTile::_jsonStatusKey = "status"; const char* TerrainTile::_jsonDataKey = "data"; const char* TerrainTile::_jsonBoundsKey = "bounds"; @@ -47,7 +47,7 @@ TerrainTile::TerrainTile(QJsonDocument document) QString errorString; QList rootVersionKeyInfoList = { { _jsonStatusKey, QJsonValue::String, true }, - { _jsonDataKey, QJsonValue::String, true }, + { _jsonDataKey, QJsonValue::Object, true }, }; if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; @@ -91,7 +91,7 @@ TerrainTile::TerrainTile(QJsonDocument document) _northEast.setLongitude(neArray[1].toDouble()); // Stats - const QJsonObject& statsObject = dataObject[_jsonBoundsKey].toObject(); + const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); QList statsVersionKeyInfoList = { { _jsonMaxElevationKey, QJsonValue::Double, true }, { _jsonMinElevationKey, QJsonValue::Double, true }, @@ -107,17 +107,17 @@ TerrainTile::TerrainTile(QJsonDocument document) // Carpet const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); - if (carpetArray.count() != _gridSize) { - qCDebug(TerrainTileLog) << "Expected array of " << _gridSize << ", instead got " << carpetArray.count(); + if (carpetArray.count() < gridSize) { // TODO (birchera): We always get 91x91 points, figure out why and where the exact location of the elev values are. + qCDebug(TerrainTileLog) << "Expected array of " << gridSize << ", instead got " << carpetArray.count(); return; } - for (int i = 0; i < _gridSize; i++) { + for (int i = 0; i < gridSize; i++) { const QJsonArray& row = carpetArray[i].toArray(); - if (row.count() != _gridSize) { - qCDebug(TerrainTileLog) << "Expected row array of " << _gridSize << ", instead got " << row.count(); + if (row.count() < gridSize) { // TODO (birchera): the same as above + qCDebug(TerrainTileLog) << "Expected row array of " << gridSize << ", instead got " << row.count(); return; } - for (int j = 0; j < _gridSize; j++) { + for (int j = 0; j < gridSize; j++) { _data[i][j] = row[j].toDouble(); } } @@ -130,8 +130,8 @@ bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; return false; } - bool ret = coordinate.latitude() >= _southWest.longitude() && coordinate.longitude() >= _southWest.longitude() && - coordinate.latitude() <= _northEast.longitude() && coordinate.longitude() <= _northEast.longitude(); + bool ret = coordinate.latitude() >= _southWest.latitude() && coordinate.longitude() >= _southWest.longitude() && + coordinate.latitude() <= _northEast.latitude() && coordinate.longitude() <= _northEast.longitude(); qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; return ret; } @@ -141,13 +141,13 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = std::round((coordinate.latitude() - _southWest.latitude()) * _gridSize / _srtm1TileSize); - int indexLon = std::round((coordinate.longitude() - _southWest.longitude()) * _gridSize / _srtm1TileSize); + int indexLat = std::round((coordinate.latitude() - _southWest.latitude()) * gridSize / srtm1TileSize); + int indexLon = std::round((coordinate.longitude() - _southWest.longitude()) * gridSize / srtm1TileSize); qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. Q_ASSERT(indexLat >= 0); - Q_ASSERT(indexLat < _gridSize); + Q_ASSERT(indexLat < gridSize); Q_ASSERT(indexLon >= 0); - Q_ASSERT(indexLon < _gridSize); + Q_ASSERT(indexLon < gridSize); qCDebug(TerrainTileLog) << "elevation" << _data[indexLat][indexLon]; return _data[indexLat][indexLon]; } else { diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 1c66cbe..199578b 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -74,10 +74,10 @@ public: QGeoCoordinate centerCoordinate(void) const; /// tile grid size in lat and lon - static const int _gridSize = TERRAIN_TILE_SIZE; + static const int gridSize = TERRAIN_TILE_SIZE; /// size of a tile in degree - static const double _srtm1TileSize; + static const double srtm1TileSize; private: QGeoCoordinate _southWest; /// South west corner of the tile From b91d729097187c40309ad0c84dd0ab19ab552451 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 16 Nov 2017 11:58:06 -0500 Subject: [PATCH 05/29] online version --- src/Terrain.cc | 84 +++++++++++++++++++++++++++++++----------------------- src/Terrain.h | 11 ++++++- src/TerrainTile.cc | 4 +-- src/TerrainTile.h | 2 +- 4 files changed, 62 insertions(+), 39 deletions(-) diff --git a/src/Terrain.cc b/src/Terrain.cc index 875a5d6..8920a5f 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -30,39 +30,9 @@ ElevationProvider::ElevationProvider(QObject* parent) } -bool ElevationProvider::queryTerrainData(const QList& coordinates) +bool ElevationProvider::queryTerrainDataPoints(const QList& coordinates) { - if (coordinates.length() == 0) { - return false; - } - - bool fallBackToOnline = false; - QList altitudes; - foreach (QGeoCoordinate coordinate, coordinates) { - QString uniqueTileId = _uniqueTileId(coordinate); - _tilesMutex.lock(); - if (!_tiles.contains(uniqueTileId)) { - _tilesMutex.unlock(); - fallBackToOnline = true; - break; - } else { - if (_tiles[uniqueTileId].isIn(coordinate)) { - altitudes.push_back(_tiles[uniqueTileId].elevation(coordinate)); - } else { - qCDebug(TerrainLog) << "Error: coordinate not in tile region"; - altitudes.push_back(-1.0); - } - } - _tilesMutex.unlock(); - } - - if (!fallBackToOnline) { - qCDebug(TerrainLog) << "All altitudes taken from cached data"; - emit terrainData(true, altitudes); - return true; - } - - if (_state != State::Idle) { + if (_state != State::Idle || coordinates.length() == 0) { return false; } @@ -95,6 +65,47 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate return true; } +bool ElevationProvider::queryTerrainData(const QList& coordinates) +{ + if (_state != State::Idle || coordinates.length() == 0) { + return false; + } + + QList altitudes; + bool needToDownload = false; + foreach (QGeoCoordinate coordinate, coordinates) { + QString uniqueTileId = _uniqueTileId(coordinate); + _tilesMutex.lock(); + if (!_tiles.contains(uniqueTileId)) { + qCDebug(TerrainLog) << "Need to download tile " << uniqueTileId; + _downloadQueue.append(uniqueTileId); + needToDownload = true; + } else { + if (!needToDownload) { + if (_tiles[uniqueTileId].isIn(coordinate)) { + altitudes.push_back(_tiles[uniqueTileId].elevation(coordinate)); + } else { + qCDebug(TerrainLog) << "Error: coordinate not in tile region"; + altitudes.push_back(-1.0); + } + } + } + _tilesMutex.unlock(); + } + + if (!needToDownload) { + qCDebug(TerrainLog) << "All altitudes taken from cached data"; + emit terrainData(true, altitudes); + _coordinates.clear(); + return true; + } + + _coordinates = coordinates; + + _downloadTiles(); + return true; +} + bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast) { qCDebug(TerrainLog) << "Cache terrain tiles southWest:northEast" << southWest << northEast; @@ -116,7 +127,7 @@ bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const _tilesMutex.unlock(); continue; } - _downloadQueue.append(uniqueTileId.replace("_", ",")); + _downloadQueue.append(uniqueTileId); _tilesMutex.unlock(); qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; } @@ -139,7 +150,7 @@ bool ElevationProvider::cacheTerrainTiles(const QList& coordinat _tilesMutex.unlock(); continue; } - _downloadQueue.append(uniqueTileId.replace("_", ",")); + _downloadQueue.append(uniqueTileId); _tilesMutex.unlock(); qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; } @@ -233,12 +244,13 @@ void ElevationProvider::_downloadTiles(void) QUrlQuery query; _tilesMutex.lock(); qCDebug(TerrainLog) << "Starting download for " << _downloadQueue.first(); - query.addQueryItem(QStringLiteral("points"), _downloadQueue.first()); + query.addQueryItem(QStringLiteral("points"), _downloadQueue.first().replace("_", ",")); _downloadQueue.pop_front(); _tilesMutex.unlock(); QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/ele/carpet")); url.setQuery(query); + qWarning() << "url" << url; QNetworkRequest request(url); QNetworkProxy tProxy; @@ -253,6 +265,8 @@ void ElevationProvider::_downloadTiles(void) connect(networkReply, &QNetworkReply::finished, this, &ElevationProvider::_requestFinishedTile); _state = State::Downloading; + } else if (_state == State::Idle) { + queryTerrainData(_coordinates); } } diff --git a/src/Terrain.h b/src/Terrain.h index 14cb99f..ede95c7 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -38,7 +38,15 @@ public: /** * Async elevation query for a list of lon,lat coordinates. When the query is done, the terrainData() signal - * is emitted. + * is emitted. This call directly looks elevations up online. + * @param coordinates + * @return true on success + */ + bool queryTerrainDataPoints(const QList& coordinates); + + /** + * Async elevation query for a list of lon,lat coordinates. When the query is done, the terrainData() signal + * is emitted. This call caches local elevation tables for faster lookup in the future. * @param coordinates * @return true on success */ @@ -80,6 +88,7 @@ private: State _state = State::Idle; QNetworkAccessManager _networkManager; + QList _coordinates; static QMutex _tilesMutex; static QHash _tiles; diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 8baca5f..fddc0ba 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -141,8 +141,8 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = std::round((coordinate.latitude() - _southWest.latitude()) * gridSize / srtm1TileSize); - int indexLon = std::round((coordinate.longitude() - _southWest.longitude()) * gridSize / srtm1TileSize); + int indexLat = round((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / srtm1TileSize); + int indexLon = round((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / srtm1TileSize); qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. Q_ASSERT(indexLat >= 0); Q_ASSERT(indexLat < gridSize); diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 199578b..4ac61de 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -5,7 +5,7 @@ #include -#define TERRAIN_TILE_SIZE 90 +#define TERRAIN_TILE_SIZE 91 Q_DECLARE_LOGGING_CATEGORY(TerrainTileLog) From e793249057ffa9d6b18a26cc0ea4b67d2d6abb9f Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 16 Nov 2017 12:06:52 -0500 Subject: [PATCH 06/29] cleanup --- src/QGCLoggingCategory.cc | 1 + src/Terrain.cc | 24 +----------------------- src/Terrain.h | 17 +++++------------ 3 files changed, 7 insertions(+), 35 deletions(-) diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index 662de9b..9372e6c 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -79,6 +79,7 @@ void QGCLoggingCategoryRegister::setFilterRulesFromSettings(const QString& comma } // Command line rules take precedence, so they go last in the list + _commandLineLoggingOptions += "TerrainLog,TerrainTileLog"; if (!_commandLineLoggingOptions.isEmpty()) { QStringList logList = _commandLineLoggingOptions.split(","); diff --git a/src/Terrain.cc b/src/Terrain.cc index 8920a5f..dd32053 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -137,28 +137,6 @@ bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const return true; } -bool ElevationProvider::cacheTerrainTiles(const QList& coordinates) -{ - if (coordinates.length() == 0) { - return false; - } - - for (const auto& coordinate : coordinates) { - QString uniqueTileId = _uniqueTileId(coordinate); - _tilesMutex.lock(); - if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { - _tilesMutex.unlock(); - continue; - } - _downloadQueue.append(uniqueTileId); - _tilesMutex.unlock(); - qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; - } - - _downloadTiles(); - return true; -} - void ElevationProvider::_requestFinished() { QNetworkReply* reply = qobject_cast(QObject::sender()); @@ -265,7 +243,7 @@ void ElevationProvider::_downloadTiles(void) connect(networkReply, &QNetworkReply::finished, this, &ElevationProvider::_requestFinishedTile); _state = State::Downloading; - } else if (_state == State::Idle) { + } else if (_state == State::Idle && _coordinates.length() > 0) { queryTerrainData(_coordinates); } } diff --git a/src/Terrain.h b/src/Terrain.h index ede95c7..286b2a0 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -53,14 +53,6 @@ public: bool queryTerrainData(const QList& coordinates); /** - * Cache all data in rectangular region given by list of coordinates. - * - * @param coordinates - * @return true on successful scheduling for download - */ - bool cacheTerrainTiles(const QList& coordinates); - - /** * Cache all data in rectangular region given by south west and north east corner. * * @param southWest @@ -70,16 +62,17 @@ public: bool cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); signals: + /// signal returning requested elevation data void terrainData(bool success, QList altitudes); private slots: - void _requestFinished(); - void _requestFinishedTile(); + void _requestFinished(); /// slot to handle download of elevation of list of coordinates + void _requestFinishedTile(); /// slot to handle download of elevation tiles private: - QString _uniqueTileId(const QGeoCoordinate& coordinate); - void _downloadTiles(void); + QString _uniqueTileId(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile. Format: south_west_north_east as floats. + void _downloadTiles(void); /// Method to trigger download of queued tiles, eventually emitting the requested altitudes (if any). enum class State { Idle, From 83643d6854ab61a8a56ee85ffdff53999f00689d Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 23 Nov 2017 17:05:02 -0500 Subject: [PATCH 07/29] Cache offline elevation maps --- src/QtLocationPlugin/QGCMapEngine.cpp | 48 ++++++++++++++++++++-- src/QtLocationPlugin/QGCMapEngine.h | 5 +++ src/QtLocationPlugin/QGCMapUrlEngine.cpp | 21 ++++++++++ src/QtLocationPlugin/QGCMapUrlEngine.h | 4 +- src/QtLocationPlugin/QMLControl/OfflineMap.qml | 8 ++++ .../QMLControl/QGCMapEngineManager.cc | 23 ++++++++++- .../QMLControl/QGCMapEngineManager.h | 8 ++-- src/Terrain.cc | 21 +++++----- src/TerrainTile.cc | 6 +-- src/TerrainTile.h | 3 -- 10 files changed, 121 insertions(+), 26 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapEngine.cpp b/src/QtLocationPlugin/QGCMapEngine.cpp index 571ce69..65fdcab 100644 --- a/src/QtLocationPlugin/QGCMapEngine.cpp +++ b/src/QtLocationPlugin/QGCMapEngine.cpp @@ -91,6 +91,12 @@ stQGeoTileCacheQGCMapTypes kEsriTypes[] = { #define NUM_ESRIMAPS (sizeof(kEsriTypes) / sizeof(stQGeoTileCacheQGCMapTypes)) +stQGeoTileCacheQGCMapTypes kElevationTypes[] = { + {"Airmap Elevation Data", UrlFactory::AirmapElevation} +}; + +#define NUM_ELEVMAPS (sizeof(kElevationTypes) / sizeof(stQGeoTileCacheQGCMapTypes)) + static const char* kMaxDiskCacheKey = "MaxDiskCache"; static const char* kMaxMemCacheKey = "MaxMemoryCache"; @@ -106,6 +112,9 @@ getQGCMapEngine() } //----------------------------------------------------------------------------- +const double QGCMapEngine::srtm1TileSize = 0.025; + +//----------------------------------------------------------------------------- void destroyMapEngine() { @@ -296,10 +305,17 @@ QGCMapEngine::getTileCount(int zoom, double topleftLon, double topleftLat, doubl if(zoom < 1) zoom = 1; if(zoom > MAX_MAP_ZOOM) zoom = MAX_MAP_ZOOM; QGCTileSet set; - set.tileX0 = long2tileX(topleftLon, zoom); - set.tileY0 = lat2tileY(topleftLat, zoom); - set.tileX1 = long2tileX(bottomRightLon, zoom); - set.tileY1 = lat2tileY(bottomRightLat, zoom); + if (mapType != UrlFactory::AirmapElevation) { + set.tileX0 = long2tileX(topleftLon, zoom); + set.tileY0 = lat2tileY(topleftLat, zoom); + set.tileX1 = long2tileX(bottomRightLon, zoom); + set.tileY1 = lat2tileY(bottomRightLat, zoom); + } else { + set.tileX0 = long2elevationTileX(topleftLon, zoom); + set.tileY0 = lat2elevationTileY(bottomRightLat, zoom); + set.tileX1 = long2elevationTileX(bottomRightLon, zoom); + set.tileY1 = lat2elevationTileY(topleftLat, zoom); + } set.tileCount = (quint64)((quint64)set.tileX1 - (quint64)set.tileX0 + 1) * (quint64)((quint64)set.tileY1 - (quint64)set.tileY0 + 1); set.tileSize = UrlFactory::averageSizeForType(mapType) * set.tileCount; return set; @@ -320,6 +336,22 @@ QGCMapEngine::lat2tileY(double lat, int z) } //----------------------------------------------------------------------------- +int +QGCMapEngine::long2elevationTileX(double lon, int z) +{ + Q_UNUSED(z); + return (int)(floor((lon + 180.0) / srtm1TileSize)); +} + +//----------------------------------------------------------------------------- +int +QGCMapEngine::lat2elevationTileY(double lat, int z) +{ + Q_UNUSED(z); + return (int)(floor((lat + 90.0) / srtm1TileSize)); +} + +//----------------------------------------------------------------------------- UrlFactory::MapType QGCMapEngine::getTypeFromName(const QString& name) { @@ -336,6 +368,10 @@ QGCMapEngine::getTypeFromName(const QString& name) if(name.compare(kEsriTypes[i].name, Qt::CaseInsensitive) == 0) return kEsriTypes[i].type; } + for(i = 0; i < NUM_ELEVMAPS; i++) { + if(name.compare(kElevationTypes[i].name, Qt::CaseInsensitive) == 0) + return kElevationTypes[i].type; + } return UrlFactory::Invalid; } @@ -357,6 +393,9 @@ QGCMapEngine::getMapNameList() mapList << kEsriTypes[i].name; } } + for(size_t i = 0; i < NUM_ELEVMAPS; i++) { + mapList << kElevationTypes[i].name; + } return mapList; } @@ -469,6 +508,7 @@ QGCMapEngine::concurrentDownloads(UrlFactory::MapType type) case UrlFactory::EsriWorldStreet: case UrlFactory::EsriWorldSatellite: case UrlFactory::EsriTerrain: + case UrlFactory::AirmapElevation: return 12; /* case UrlFactory::MapQuestMap: diff --git a/src/QtLocationPlugin/QGCMapEngine.h b/src/QtLocationPlugin/QGCMapEngine.h index e0fd280..352fcbd 100644 --- a/src/QtLocationPlugin/QGCMapEngine.h +++ b/src/QtLocationPlugin/QGCMapEngine.h @@ -94,12 +94,17 @@ public: static QGCTileSet getTileCount (int zoom, double topleftLon, double topleftLat, double bottomRightLon, double bottomRightLat, UrlFactory::MapType mapType); static int long2tileX (double lon, int z); static int lat2tileY (double lat, int z); + static int long2elevationTileX (double lon, int z); + static int lat2elevationTileY (double lat, int z); static QString getTileHash (UrlFactory::MapType type, int x, int y, int z); static UrlFactory::MapType getTypeFromName (const QString &name); static QString bigSizeToString (quint64 size); static QString numberToString (quint64 number); static int concurrentDownloads (UrlFactory::MapType type); + /// size of an elevation tile in degree + static const double srtm1TileSize; + private slots: void _updateTotals (quint32 totaltiles, quint64 totalsize, quint32 defaulttiles, quint64 defaultsize); void _pruned (); diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index b429cd5..b610791 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -36,6 +36,7 @@ static const unsigned char pngSignature[] = {0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A, 0x00}; static const unsigned char jpegSignature[] = {0xFF, 0xD8, 0xFF, 0x00}; static const unsigned char gifSignature[] = {0x47, 0x49, 0x46, 0x38, 0x00}; +static const unsigned char jsonSignature[] = {0x7A, 0x22, 0x00}; // two characters '{"' //----------------------------------------------------------------------------- UrlFactory::UrlFactory() @@ -85,6 +86,8 @@ UrlFactory::getImageFormat(MapType type, const QByteArray& image) format = "jpg"; else if (image.startsWith(reinterpret_cast(gifSignature))) format = "gif"; + else if (image.startsWith(reinterpret_cast(jsonSignature))) + format = "json"; else { switch (type) { case GoogleMap: @@ -119,6 +122,9 @@ UrlFactory::getImageFormat(MapType type, const QByteArray& image) case BingHybrid: format = "jpg"; break; + case AirmapElevation: + format = "json"; + break; default: qWarning("UrlFactory::getImageFormat() Unknown map id %d", type); break; @@ -177,6 +183,10 @@ UrlFactory::getTileURL(MapType type, int x, int y, int zoom, QNetworkAccessManag } return request; + case AirmapElevation: + request.setRawHeader("Referrer", "https://api.airmap.com/"); + break; + default: break; } @@ -392,6 +402,14 @@ UrlFactory::_getURL(MapType type, int x, int y, int zoom, QNetworkAccessManager* } } break; + case AirmapElevation: + { + return QString("https://api.airmap.com/elevation/stage/srtm1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( + static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0).arg( + static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0); + } + break; default: qWarning("Unknown map id %d\n", type); @@ -534,6 +552,7 @@ UrlFactory::_tryCorrectGoogleVersions(QNetworkAccessManager* networkManager) #define AVERAGE_MAPBOX_SAT_MAP 15739 #define AVERAGE_MAPBOX_STREET_MAP 5648 #define AVERAGE_TILE_SIZE 13652 +#define AVERAGE_AIRMAP_ELEV_SIZE 34000 //----------------------------------------------------------------------------- quint32 @@ -557,6 +576,8 @@ UrlFactory::averageSizeForType(MapType type) case MapboxStreetsBasic: case MapboxRunBikeHike: return AVERAGE_MAPBOX_STREET_MAP; + case AirmapElevation: + return AVERAGE_AIRMAP_ELEV_SIZE; case GoogleLabels: case MapboxDark: case MapboxLight: diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.h b/src/QtLocationPlugin/QGCMapUrlEngine.h index b9791ff..87cf64e 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.h +++ b/src/QtLocationPlugin/QGCMapUrlEngine.h @@ -72,7 +72,9 @@ public: EsriWorldStreet = 7000, EsriWorldSatellite = 7001, - EsriTerrain = 7002 + EsriTerrain = 7002, + + AirmapElevation = 8000 }; UrlFactory (); diff --git a/src/QtLocationPlugin/QMLControl/OfflineMap.qml b/src/QtLocationPlugin/QMLControl/OfflineMap.qml index e2d336c..cd132a5 100644 --- a/src/QtLocationPlugin/QMLControl/OfflineMap.qml +++ b/src/QtLocationPlugin/QMLControl/OfflineMap.qml @@ -732,6 +732,14 @@ QGCView { } } } + QGCCheckBox { + anchors.left: parent.left + anchors.right: parent.right + text: qsTr("Fetch elevation data") + checked: QGroundControl.mapEngineManager.fetchElevation + onClicked: QGroundControl.mapEngineManager.fetchElevation = checked + visible: mapType != "Airmap Elevation Data" + } } Rectangle { diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index fce1e12..ea64716 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -41,6 +41,7 @@ QGCMapEngineManager::QGCMapEngineManager(QGCApplication* app, QGCToolbox* toolbo , _setID(UINT64_MAX) , _freeDiskSpace(0) , _diskSpace(0) + , _fetchElevation(true) , _actionProgress(0) , _importAction(ActionNone) , _importReplace(false) @@ -157,8 +158,26 @@ QGCMapEngineManager::startDownload(const QString& name, const QString& mapType) } else { qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; } - // TODO: this could also get some feedback - _elevationProvider.cacheTerrainTiles(QGeoCoordinate(_bottomRightLat, _topleftLon), QGeoCoordinate(_topleftLat, _bottomRightLon)); + if (mapType != "Airmap Elevation Data" && _fetchElevation) { + QGCCachedTileSet* set = new QGCCachedTileSet(name + " Elevation"); + set->setMapTypeStr("Airmap Elevation Data"); + set->setTopleftLat(_topleftLat); + set->setTopleftLon(_topleftLon); + set->setBottomRightLat(_bottomRightLat); + set->setBottomRightLon(_bottomRightLon); + set->setMinZoom(1); + set->setMaxZoom(1); + set->setTotalTileSize(_totalSet.tileSize); + set->setTotalTileCount(_totalSet.tileCount); + set->setType(QGCMapEngine::getTypeFromName("Airmap Elevation Data")); + QGCCreateTileSetTask* task = new QGCCreateTileSetTask(set); + //-- Create Tile Set (it will also create a list of tiles to download) + connect(task, &QGCCreateTileSetTask::tileSetSaved, this, &QGCMapEngineManager::_tileSetSaved); + connect(task, &QGCMapTask::error, this, &QGCMapEngineManager::taskError); + getQGCMapEngine()->addTask(task); + } else { + qWarning() << "QGCMapEngineManager::startDownload() No Tiles to save"; + } } //----------------------------------------------------------------------------- diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h index 73c9b73..3a5d283 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h @@ -19,7 +19,6 @@ #include "QGCLoggingCategory.h" #include "QGCMapEngine.h" #include "QGCMapTileSet.h" -#include "Terrain.h" Q_DECLARE_LOGGING_CATEGORY(QGCMapEngineManagerLog) @@ -51,6 +50,7 @@ public: Q_PROPERTY(quint32 maxMemCache READ maxMemCache WRITE setMaxMemCache NOTIFY maxMemCacheChanged) Q_PROPERTY(quint32 maxDiskCache READ maxDiskCache WRITE setMaxDiskCache NOTIFY maxDiskCacheChanged) Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) + Q_PROPERTY(bool fetchElevation READ fetchElevation WRITE setFetchElevation NOTIFY fetchElevationChanged) //-- Disk Space in MB Q_PROPERTY(quint32 freeDiskSpace READ freeDiskSpace NOTIFY freeDiskSpaceChanged) Q_PROPERTY(quint32 diskSpace READ diskSpace CONSTANT) @@ -89,6 +89,7 @@ public: quint32 maxMemCache (); quint32 maxDiskCache (); QString errorMessage () { return _errorMessage; } + bool fetchElevation () { return _fetchElevation; } quint64 freeDiskSpace () { return _freeDiskSpace; } quint64 diskSpace () { return _diskSpace; } int selectedCount (); @@ -101,6 +102,7 @@ public: void setImportReplace (bool replace) { _importReplace = replace; emit importReplaceChanged(); } void setImportAction (ImportAction action) {_importAction = action; emit importActionChanged(); } void setErrorMessage (const QString& error) { _errorMessage = error; emit errorMessageChanged(); } + void setFetchElevation (bool fetchElevation) { _fetchElevation = fetchElevation; emit fetchElevationChanged(); } // Override from QGCTool void setToolbox(QGCToolbox *toolbox); @@ -116,6 +118,7 @@ signals: void maxMemCacheChanged (); void maxDiskCacheChanged (); void errorMessageChanged (); + void fetchElevationChanged (); void freeDiskSpaceChanged (); void selectedCountChanged (); void actionProgressChanged (); @@ -150,11 +153,10 @@ private: quint32 _diskSpace; QmlObjectListModel _tileSets; QString _errorMessage; + bool _fetchElevation; int _actionProgress; ImportAction _importAction; bool _importReplace; - - ElevationProvider _elevationProvider; }; #endif diff --git a/src/Terrain.cc b/src/Terrain.cc index dd32053..988842f 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -8,6 +8,7 @@ ****************************************************************************/ #include "Terrain.h" +#include "QGCMapEngine.h" #include #include @@ -112,15 +113,15 @@ bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const // Correct corners of the tile to fixed grid QGeoCoordinate southWestCorrected; - southWestCorrected.setLatitude(floor(southWest.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); - southWestCorrected.setLongitude(floor(southWest.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + southWestCorrected.setLatitude(floor(southWest.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); + southWestCorrected.setLongitude(floor(southWest.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); QGeoCoordinate northEastCorrected; - northEastCorrected.setLatitude(ceil(northEast.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); - northEastCorrected.setLongitude(ceil(northEast.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + northEastCorrected.setLatitude(ceil(northEast.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); + northEastCorrected.setLongitude(ceil(northEast.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); // Add all tiles to download queue - for (double lat = southWestCorrected.latitude() + TerrainTile::srtm1TileSize / 2.0; lat < northEastCorrected.latitude(); lat += TerrainTile::srtm1TileSize) { - for (double lon = southWestCorrected.longitude() + TerrainTile::srtm1TileSize / 2.0; lon < northEastCorrected.longitude(); lon += TerrainTile::srtm1TileSize) { + for (double lat = southWestCorrected.latitude() + QGCMapEngine::srtm1TileSize / 2.0; lat < northEastCorrected.latitude(); lat += QGCMapEngine::srtm1TileSize) { + for (double lon = southWestCorrected.longitude() + QGCMapEngine::srtm1TileSize / 2.0; lon < northEastCorrected.longitude(); lon += QGCMapEngine::srtm1TileSize) { QString uniqueTileId = _uniqueTileId(QGeoCoordinate(lat, lon)); _tilesMutex.lock(); if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { @@ -252,11 +253,11 @@ QString ElevationProvider::_uniqueTileId(const QGeoCoordinate& coordinate) { // Compute corners of the tile QGeoCoordinate southWest; - southWest.setLatitude(floor(coordinate.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); - southWest.setLongitude(floor(coordinate.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + southWest.setLatitude(floor(coordinate.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); + southWest.setLongitude(floor(coordinate.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); QGeoCoordinate northEast; - northEast.setLatitude(ceil(coordinate.latitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); - northEast.setLongitude(ceil(coordinate.longitude()/TerrainTile::srtm1TileSize)*TerrainTile::srtm1TileSize); + northEast.setLatitude(floor(coordinate.latitude()/QGCMapEngine::srtm1TileSize + 1)*QGCMapEngine::srtm1TileSize); + northEast.setLongitude(floor(coordinate.longitude()/QGCMapEngine::srtm1TileSize + 1)*QGCMapEngine::srtm1TileSize); // Generate uniquely identifying string QString ret = QString::number(southWest.latitude(), 'f', 6) + "_" + QString::number(southWest.longitude(), 'f', 6) + "_" + diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index fddc0ba..38a1d27 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -1,5 +1,6 @@ #include "TerrainTile.h" #include "JsonHelper.h" +#include "QGCMapEngine.h" #include #include @@ -7,7 +8,6 @@ QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") -const double TerrainTile::srtm1TileSize = 0.025; const char* TerrainTile::_jsonStatusKey = "status"; const char* TerrainTile::_jsonDataKey = "data"; const char* TerrainTile::_jsonBoundsKey = "bounds"; @@ -141,8 +141,8 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = round((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / srtm1TileSize); - int indexLon = round((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / srtm1TileSize); + int indexLat = round((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize); + int indexLon = round((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize); qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. Q_ASSERT(indexLat >= 0); Q_ASSERT(indexLat < gridSize); diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 4ac61de..3b757b9 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -76,9 +76,6 @@ public: /// tile grid size in lat and lon static const int gridSize = TERRAIN_TILE_SIZE; - /// size of a tile in degree - static const double srtm1TileSize; - private: QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile From 5fb4a54fad3e2001c7ce1478b4080fa4ee9f1d73 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 23 Nov 2017 21:17:11 -0500 Subject: [PATCH 08/29] load tiles from cache if available --- src/QtLocationPlugin/QGeoMapReplyQGC.cpp | 3 + src/Terrain.cc | 155 ++++++++++--------------------- src/Terrain.h | 16 +--- src/TerrainTile.cc | 8 +- 4 files changed, 60 insertions(+), 122 deletions(-) diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index 2c88231..ff47fc0 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -112,9 +112,11 @@ QGeoTiledMapReplyQGC::networkReplyFinished() { _timer.stop(); if (!_reply) { + abort(); return; } if (_reply->error() != QNetworkReply::NoError) { + abort(); return; } QByteArray a = _reply->readAll(); @@ -195,4 +197,5 @@ QGeoTiledMapReplyQGC::timeout() if(_reply) { _reply->abort(); } + abort(); } diff --git a/src/Terrain.cc b/src/Terrain.cc index 988842f..64d878e 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -9,6 +9,8 @@ #include "Terrain.h" #include "QGCMapEngine.h" +#include "QGeoMapReplyQGC.h" +#include #include #include @@ -75,16 +77,31 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate QList altitudes; bool needToDownload = false; foreach (QGeoCoordinate coordinate, coordinates) { - QString uniqueTileId = _uniqueTileId(coordinate); + QString tileHash = _getTileHash(coordinate); _tilesMutex.lock(); - if (!_tiles.contains(uniqueTileId)) { - qCDebug(TerrainLog) << "Need to download tile " << uniqueTileId; - _downloadQueue.append(uniqueTileId); + if (!_tiles.contains(tileHash)) { + qCDebug(TerrainLog) << "Need to download tile " << tileHash; + + if (!_downloadQueue.contains(tileHash)) { + // Schedule the fetch task + + QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); + QGeoTileSpec spec; + spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); + spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); + spec.setZoom(1); + spec.setMapId(UrlFactory::AirmapElevation); + QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); + connect(reply, &QGeoTiledMapReplyQGC::finished, this, &ElevationProvider::_fetchedTile); + connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &ElevationProvider::_fetchedTile); + + _downloadQueue.append(tileHash); + } needToDownload = true; } else { if (!needToDownload) { - if (_tiles[uniqueTileId].isIn(coordinate)) { - altitudes.push_back(_tiles[uniqueTileId].elevation(coordinate)); + if (_tiles[tileHash].isIn(coordinate)) { + altitudes.push_back(_tiles[tileHash].elevation(coordinate)); } else { qCDebug(TerrainLog) << "Error: coordinate not in tile region"; altitudes.push_back(-1.0); @@ -103,39 +120,7 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate _coordinates = coordinates; - _downloadTiles(); - return true; -} - -bool ElevationProvider::cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast) -{ - qCDebug(TerrainLog) << "Cache terrain tiles southWest:northEast" << southWest << northEast; - - // Correct corners of the tile to fixed grid - QGeoCoordinate southWestCorrected; - southWestCorrected.setLatitude(floor(southWest.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - southWestCorrected.setLongitude(floor(southWest.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - QGeoCoordinate northEastCorrected; - northEastCorrected.setLatitude(ceil(northEast.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - northEastCorrected.setLongitude(ceil(northEast.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - - // Add all tiles to download queue - for (double lat = southWestCorrected.latitude() + QGCMapEngine::srtm1TileSize / 2.0; lat < northEastCorrected.latitude(); lat += QGCMapEngine::srtm1TileSize) { - for (double lon = southWestCorrected.longitude() + QGCMapEngine::srtm1TileSize / 2.0; lon < northEastCorrected.longitude(); lon += QGCMapEngine::srtm1TileSize) { - QString uniqueTileId = _uniqueTileId(QGeoCoordinate(lat, lon)); - _tilesMutex.lock(); - if (_downloadQueue.contains(uniqueTileId) || _tiles.contains(uniqueTileId)) { - _tilesMutex.unlock(); - continue; - } - _downloadQueue.append(uniqueTileId); - _tilesMutex.unlock(); - qCDebug(TerrainLog) << "Adding tile to download queue: " << uniqueTileId; - } - } - - _downloadTiles(); - return true; + return false; } void ElevationProvider::_requestFinished() @@ -176,93 +161,51 @@ void ElevationProvider::_requestFinished() emit terrainData(false, altitudes); } -void ElevationProvider::_requestFinishedTile() +void ElevationProvider::_fetchedTile() { - QNetworkReply* reply = qobject_cast(QObject::sender()); - _state = State::Idle; + QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); - // When an error occurs we still end up here - if (reply->error() != QNetworkReply::NoError) { - QByteArray responseBytes = reply->readAll(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - qCDebug(TerrainLog) << "ERROR: Received " << responseJson; - // TODO (birchera): Handle error in downloading data - _downloadTiles(); + if (!reply || !reply->isFinished()) { + if (reply) { + qCDebug(TerrainLog) << "Error in fetching elevation tile: " << reply->errorString(); + reply->deleteLater(); + } else { + qCDebug(TerrainLog) << "Elevation tile fetched but invalid reply data type."; + } return; } - QByteArray responseBytes = reply->readAll(); + QByteArray responseBytes = reply->mapImageData(); QJsonParseError parseError; QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); if (parseError.error != QJsonParseError::NoError) { - // TODO (birchera): Handle error in downloading data - _downloadTiles(); + qCDebug(TerrainLog) << "Could not parse terrain tile " << parseError.errorString(); + qCDebug(TerrainLog) << responseBytes; + reply->deleteLater(); return; } - TerrainTile* tile = new TerrainTile(responseJson); - if (tile->isValid()) { + TerrainTile* terrainTile = new TerrainTile(responseJson); + if (terrainTile->isValid()) { _tilesMutex.lock(); - if (!_tiles.contains(_uniqueTileId(tile->centerCoordinate()))) { - _tiles.insert(_uniqueTileId(tile->centerCoordinate()), *tile); + if (!_tiles.contains(_getTileHash(terrainTile->centerCoordinate()))) { + _tiles.insert(_getTileHash(terrainTile->centerCoordinate()), *terrainTile); } else { - delete tile; + delete terrainTile; } - _tilesMutex.unlock(); - } - - _downloadTiles(); -} - -void ElevationProvider::_downloadTiles(void) -{ - if (_state == State::Idle && _downloadQueue.count() > 0) { - QUrlQuery query; - _tilesMutex.lock(); - qCDebug(TerrainLog) << "Starting download for " << _downloadQueue.first(); - query.addQueryItem(QStringLiteral("points"), _downloadQueue.first().replace("_", ",")); - _downloadQueue.pop_front(); - _tilesMutex.unlock(); - QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/ele/carpet")); - url.setQuery(query); - - qWarning() << "url" << url; - QNetworkRequest request(url); - - QNetworkProxy tProxy; - tProxy.setType(QNetworkProxy::DefaultProxy); - _networkManager.setProxy(tProxy); - - QNetworkReply* networkReply = _networkManager.get(request); - if (!networkReply) { - return; + if (_downloadQueue.contains(_getTileHash(terrainTile->centerCoordinate()))) { + _downloadQueue.removeOne(_getTileHash(terrainTile->centerCoordinate())); } - - connect(networkReply, &QNetworkReply::finished, this, &ElevationProvider::_requestFinishedTile); - - _state = State::Downloading; - } else if (_state == State::Idle && _coordinates.length() > 0) { - queryTerrainData(_coordinates); + _tilesMutex.unlock(); } + reply->deleteLater(); } -QString ElevationProvider::_uniqueTileId(const QGeoCoordinate& coordinate) +QString ElevationProvider::_getTileHash(const QGeoCoordinate& coordinate) { - // Compute corners of the tile - QGeoCoordinate southWest; - southWest.setLatitude(floor(coordinate.latitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - southWest.setLongitude(floor(coordinate.longitude()/QGCMapEngine::srtm1TileSize)*QGCMapEngine::srtm1TileSize); - QGeoCoordinate northEast; - northEast.setLatitude(floor(coordinate.latitude()/QGCMapEngine::srtm1TileSize + 1)*QGCMapEngine::srtm1TileSize); - northEast.setLongitude(floor(coordinate.longitude()/QGCMapEngine::srtm1TileSize + 1)*QGCMapEngine::srtm1TileSize); - - // Generate uniquely identifying string - QString ret = QString::number(southWest.latitude(), 'f', 6) + "_" + QString::number(southWest.longitude(), 'f', 6) + "_" + - QString::number(northEast.latitude(), 'f', 6) + "_" + QString::number(northEast.longitude(), 'f', 6); - qCDebug(TerrainLog) << "Computing unique tile id for " << coordinate << ret; + QString ret = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1); + qCDebug(TerrainLog) << "Computing unique tile hash for " << coordinate << ret; return ret; } diff --git a/src/Terrain.h b/src/Terrain.h index 286b2a0..742d0d3 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -10,6 +10,7 @@ #pragma once #include "TerrainTile.h" +#include "QGCMapEngineData.h" #include "QGCLoggingCategory.h" #include @@ -17,6 +18,7 @@ #include #include #include +#include /* usage example: ElevationProvider *p = new ElevationProvider(); @@ -52,27 +54,17 @@ public: */ bool queryTerrainData(const QList& coordinates); - /** - * Cache all data in rectangular region given by south west and north east corner. - * - * @param southWest - * @param northEast - * @return true on successful scheduling for download - */ - bool cacheTerrainTiles(const QGeoCoordinate& southWest, const QGeoCoordinate& northEast); - signals: /// signal returning requested elevation data void terrainData(bool success, QList altitudes); private slots: void _requestFinished(); /// slot to handle download of elevation of list of coordinates - void _requestFinishedTile(); /// slot to handle download of elevation tiles + void _fetchedTile(); /// slot to handle fetched elevation tiles private: - QString _uniqueTileId(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile. Format: south_west_north_east as floats. - void _downloadTiles(void); /// Method to trigger download of queued tiles, eventually emitting the requested altitudes (if any). + QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile. Format: south_west_north_east as floats. enum class State { Idle, diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 38a1d27..d52ccd2 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -85,10 +85,10 @@ TerrainTile::TerrainTile(QJsonDocument document) qCDebug(TerrainTileLog) << "Incomplete bounding location"; return; } - _southWest.setLatitude(swArray[0].toDouble()); - _southWest.setLongitude(swArray[1].toDouble()); - _northEast.setLatitude(neArray[0].toDouble()); - _northEast.setLongitude(neArray[1].toDouble()); + _southWest.setLatitude(swArray[1].toDouble()); + _southWest.setLongitude(swArray[0].toDouble()); + _northEast.setLatitude(neArray[1].toDouble()); + _northEast.setLongitude(neArray[0].toDouble()); // Stats const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); From 753bafa78fc2fc43930ef96c72bd6f71da88732f Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 24 Nov 2017 11:17:53 -0500 Subject: [PATCH 09/29] error handling --- src/QtLocationPlugin/QGeoMapReplyQGC.cpp | 7 +++--- src/Terrain.cc | 39 ++++++++++++++++++++++++-------- 2 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index ff47fc0..d95f1db 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -104,6 +104,7 @@ QGeoTiledMapReplyQGC::abort() _timer.stop(); if (_reply) _reply->abort(); + emit aborted(); } //----------------------------------------------------------------------------- @@ -112,11 +113,11 @@ QGeoTiledMapReplyQGC::networkReplyFinished() { _timer.stop(); if (!_reply) { - abort(); + emit aborted(); return; } if (_reply->error() != QNetworkReply::NoError) { - abort(); + emit aborted(); return; } QByteArray a = _reply->readAll(); @@ -197,5 +198,5 @@ QGeoTiledMapReplyQGC::timeout() if(_reply) { _reply->abort(); } - abort(); + emit aborted(); } diff --git a/src/Terrain.cc b/src/Terrain.cc index 64d878e..c0fb92c 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -10,7 +10,6 @@ #include "Terrain.h" #include "QGCMapEngine.h" #include "QGeoMapReplyQGC.h" -#include #include #include @@ -20,6 +19,7 @@ #include #include #include +#include QGC_LOGGING_CATEGORY(TerrainLog, "TerrainLog") @@ -165,16 +165,37 @@ void ElevationProvider::_fetchedTile() { QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); - if (!reply || !reply->isFinished()) { - if (reply) { - qCDebug(TerrainLog) << "Error in fetching elevation tile: " << reply->errorString(); - reply->deleteLater(); + if (!reply) { + qCDebug(TerrainLog) << "Elevation tile fetched but invalid reply data type."; + return; + } + + // remove from download queue + QGeoTileSpec spec = reply->tileSpec(); + QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); + _tilesMutex.lock(); + if (_downloadQueue.contains(hash)) { + _downloadQueue.removeOne(hash); + } + _tilesMutex.unlock(); + + // handle potential errors + if (reply->error() != QGeoTiledMapReply::NoError) { + if (reply->error() == QGeoTiledMapReply::CommunicationError) { + qCDebug(TerrainLog) << "Elevation tile fetching returned communication error. " << reply->errorString(); } else { - qCDebug(TerrainLog) << "Elevation tile fetched but invalid reply data type."; + qCDebug(TerrainLog) << "Elevation tile fetching returned error. " << reply->errorString(); } + reply->deleteLater(); + return; + } + if (!reply->isFinished()) { + qCDebug(TerrainLog) << "Error in fetching elevation tile. Not finished. " << reply->errorString(); + reply->deleteLater(); return; } + // parse received data and insert into hash table QByteArray responseBytes = reply->mapImageData(); QJsonParseError parseError; @@ -194,12 +215,12 @@ void ElevationProvider::_fetchedTile() } else { delete terrainTile; } - if (_downloadQueue.contains(_getTileHash(terrainTile->centerCoordinate()))) { - _downloadQueue.removeOne(_getTileHash(terrainTile->centerCoordinate())); - } _tilesMutex.unlock(); } reply->deleteLater(); + + // now try to query the data again + queryTerrainData(_coordinates); } QString ElevationProvider::_getTileHash(const QGeoCoordinate& coordinate) From e311465d28b13a6670ca961bf2bb849155818c42 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Mon, 27 Nov 2017 17:15:19 -0500 Subject: [PATCH 10/29] minor fixes --- src/QtLocationPlugin/QGCMapUrlEngine.cpp | 8 ++++---- src/Terrain.cc | 6 ++++-- src/TerrainTile.cc | 8 ++++---- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index b610791..fe2a8a2 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -404,10 +404,10 @@ UrlFactory::_getURL(MapType type, int x, int y, int zoom, QNetworkAccessManager* break; case AirmapElevation: { - return QString("https://api.airmap.com/elevation/stage/srtm1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( - static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( - static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0).arg( - static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0); + return QString("https://api.airmap.com/elevation/stage/srtm1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( + static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0); } break; diff --git a/src/Terrain.cc b/src/Terrain.cc index c0fb92c..bf5fced 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -210,12 +210,14 @@ void ElevationProvider::_fetchedTile() TerrainTile* terrainTile = new TerrainTile(responseJson); if (terrainTile->isValid()) { _tilesMutex.lock(); - if (!_tiles.contains(_getTileHash(terrainTile->centerCoordinate()))) { - _tiles.insert(_getTileHash(terrainTile->centerCoordinate()), *terrainTile); + if (!_tiles.contains(hash)) { + _tiles.insert(hash), *terrainTile); } else { delete terrainTile; } _tilesMutex.unlock(); + } else { + qCDebug(TerrainLog) << "Received invalid tile"; } reply->deleteLater(); diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index d52ccd2..38a1d27 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -85,10 +85,10 @@ TerrainTile::TerrainTile(QJsonDocument document) qCDebug(TerrainTileLog) << "Incomplete bounding location"; return; } - _southWest.setLatitude(swArray[1].toDouble()); - _southWest.setLongitude(swArray[0].toDouble()); - _northEast.setLatitude(neArray[1].toDouble()); - _northEast.setLongitude(neArray[0].toDouble()); + _southWest.setLatitude(swArray[0].toDouble()); + _southWest.setLongitude(swArray[1].toDouble()); + _northEast.setLatitude(neArray[0].toDouble()); + _northEast.setLongitude(neArray[1].toDouble()); // Stats const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); From 5d3b9f611cc7d28cccea9de982b48949e7a77848 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 28 Nov 2017 07:37:15 -0500 Subject: [PATCH 11/29] build it --- src/QtLocationPlugin/QGCMapUrlEngine.cpp | 2 +- src/Terrain.cc | 2 +- src/Terrain.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index fe2a8a2..95c6de3 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -36,7 +36,7 @@ static const unsigned char pngSignature[] = {0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A, 0x00}; static const unsigned char jpegSignature[] = {0xFF, 0xD8, 0xFF, 0x00}; static const unsigned char gifSignature[] = {0x47, 0x49, 0x46, 0x38, 0x00}; -static const unsigned char jsonSignature[] = {0x7A, 0x22, 0x00}; // two characters '{"' +static const unsigned char jsonSignature[] = {0x7B, 0x22, 0x00}; // two characters '{"' //----------------------------------------------------------------------------- UrlFactory::UrlFactory() diff --git a/src/Terrain.cc b/src/Terrain.cc index bf5fced..9d4f620 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -211,7 +211,7 @@ void ElevationProvider::_fetchedTile() if (terrainTile->isValid()) { _tilesMutex.lock(); if (!_tiles.contains(hash)) { - _tiles.insert(hash), *terrainTile); + _tiles.insert(hash, *terrainTile); } else { delete terrainTile; } diff --git a/src/Terrain.h b/src/Terrain.h index 742d0d3..9fe154f 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -64,7 +64,7 @@ private slots: private: - QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile. Format: south_west_north_east as floats. + QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile enum class State { Idle, From f632bb74a6c64af368621f7f5aab2543816e1243 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 28 Nov 2017 09:35:27 -0500 Subject: [PATCH 12/29] make ci happy --- src/TerrainTile.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 38a1d27..7fd390f 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -141,8 +141,8 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = round((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize); - int indexLon = round((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize); + int indexLat = qRound(static_cast((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize)); + int indexLon = qRound(static_cast((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize)); qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. Q_ASSERT(indexLat >= 0); Q_ASSERT(indexLat < gridSize); From 6467f65365381f3c13d4890d28b47b97bc1883fe Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Mon, 4 Dec 2017 14:44:32 -0500 Subject: [PATCH 13/29] correct localization of the data points Conflicts: src/ElevationProfiler.cc --- src/TerrainTile.cc | 64 ++++++++++++++++++++++++++++++++++++++++-------------- src/TerrainTile.h | 16 ++++++++++---- 2 files changed, 60 insertions(+), 20 deletions(-) diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 7fd390f..878497f 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -23,6 +23,9 @@ TerrainTile::TerrainTile() : _minElevation(-1.0) , _maxElevation(-1.0) , _avgElevation(-1.0) + , _data(NULL) + , _gridSizeLat(-1) + , _gridSizeLon(-1) , _isValid(false) { @@ -30,6 +33,13 @@ TerrainTile::TerrainTile() TerrainTile::~TerrainTile() { + if (_data) { + for (int i = 0; i < _gridSizeLat; i++) { + delete _data[i]; + delete _data; + _data = NULL; + } + } } TerrainTile::TerrainTile(QJsonDocument document) @@ -107,17 +117,25 @@ TerrainTile::TerrainTile(QJsonDocument document) // Carpet const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); - if (carpetArray.count() < gridSize) { // TODO (birchera): We always get 91x91 points, figure out why and where the exact location of the elev values are. - qCDebug(TerrainTileLog) << "Expected array of " << gridSize << ", instead got " << carpetArray.count(); - return; - } - for (int i = 0; i < gridSize; i++) { + _gridSizeLat = carpetArray.count(); + qCDebug(TerrainTileLog) << "Received tile has size in latitude direction: " << carpetArray.count(); + for (int i = 0; i < _gridSizeLat; i++) { const QJsonArray& row = carpetArray[i].toArray(); - if (row.count() < gridSize) { // TODO (birchera): the same as above - qCDebug(TerrainTileLog) << "Expected row array of " << gridSize << ", instead got " << row.count(); + if (i == 0) { + _gridSizeLon = row.count(); + qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << row.count(); + if (_gridSizeLon > 0) { + _data = new float*[_gridSizeLat]; + } + for (int k = 0; k < _gridSizeLat; k++) { + _data[k] = new float[_gridSizeLon]; + } + } + if (row.count() < _gridSizeLon) { + qCDebug(TerrainTileLog) << "Expected row array of " << _gridSizeLon << ", instead got " << row.count(); return; } - for (int j = 0; j < gridSize; j++) { + for (int j = 0; j < _gridSizeLon; j++) { _data[i][j] = row[j].toDouble(); } } @@ -141,14 +159,10 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; // Get the index at resolution of 1 arc second - int indexLat = qRound(static_cast((coordinate.latitude() - _southWest.latitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize)); - int indexLon = qRound(static_cast((coordinate.longitude() - _southWest.longitude()) * (gridSize - 1) / QGCMapEngine::srtm1TileSize)); - qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; // TODO (birchera): Move this down to the next debug output, once this is all properly working. - Q_ASSERT(indexLat >= 0); - Q_ASSERT(indexLat < gridSize); - Q_ASSERT(indexLon >= 0); - Q_ASSERT(indexLon < gridSize); - qCDebug(TerrainTileLog) << "elevation" << _data[indexLat][indexLon]; + int indexLat = _latToDataIndex(coordinate.latitude()); + int indexLon = _lonToDataIndex(coordinate.longitude()); + qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; + qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; return _data[indexLat][indexLon]; } else { qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; @@ -160,3 +174,21 @@ QGeoCoordinate TerrainTile::centerCoordinate(void) const { return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); } + +int TerrainTile::_latToDataIndex(double latitude) const +{ + if (isValid() && _southWest.isValid() && _northEast.isValid()) { + return qRound((latitude - _southWest.latitude()) / (_northEast.latitude() - _southWest.latitude()) * (_gridSizeLat - 1)); + } else { + return -1; + } +} + +int TerrainTile::_lonToDataIndex(double longitude) const +{ + if (isValid() && _southWest.isValid() && _northEast.isValid()) { + return qRound((longitude - _southWest.longitude()) / (_northEast.longitude() - _southWest.longitude()) * (_gridSizeLon - 1)); + } else { + return -1; + } +} diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 3b757b9..ccc6ac8 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -9,6 +9,12 @@ Q_DECLARE_LOGGING_CATEGORY(TerrainTileLog) +/** + * @brief The TerrainTile class + * + * Implements an interface for https://developers.airmap.com/v2.0/docs/elevation-api + */ + class TerrainTile { public: @@ -73,10 +79,10 @@ public: */ QGeoCoordinate centerCoordinate(void) const; - /// tile grid size in lat and lon - static const int gridSize = TERRAIN_TILE_SIZE; - private: + inline int _latToDataIndex(double latitude) const; + inline int _lonToDataIndex(double longitude) const; + QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile @@ -84,7 +90,9 @@ private: float _maxElevation; /// Maximum elevation in tile float _avgElevation; /// Average elevation of the tile - float _data[TERRAIN_TILE_SIZE][TERRAIN_TILE_SIZE]; /// elevation data + float** _data; /// 2D elevation data array + int _gridSizeLat; /// data grid size in latitude direction + int _gridSizeLon; /// data grid size in longitude direction bool _isValid; /// data loaded is valid // Json keys From 07bb23653d81c2a5836c889abb7fe98e46415483 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Mon, 4 Dec 2017 15:16:25 -0500 Subject: [PATCH 14/29] cleanup --- src/QGCLoggingCategory.cc | 1 - src/TerrainTile.cc | 1 - 2 files changed, 2 deletions(-) diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index 9372e6c..662de9b 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -79,7 +79,6 @@ void QGCLoggingCategoryRegister::setFilterRulesFromSettings(const QString& comma } // Command line rules take precedence, so they go last in the list - _commandLineLoggingOptions += "TerrainLog,TerrainTileLog"; if (!_commandLineLoggingOptions.isEmpty()) { QStringList logList = _commandLineLoggingOptions.split(","); diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 878497f..d1b6394 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -161,7 +161,6 @@ float TerrainTile::elevation(const QGeoCoordinate& coordinate) const // Get the index at resolution of 1 arc second int indexLat = _latToDataIndex(coordinate.latitude()); int indexLon = _lonToDataIndex(coordinate.longitude()); - qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon; qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; return _data[indexLat][indexLon]; } else { From 1f27b9f1c372b5194136c62316d9d57cdd948813 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 15 Dec 2017 11:06:55 -0500 Subject: [PATCH 15/29] smaller tiles and only allow offline caching downloading together with map tiles --- src/QtLocationPlugin/QGCMapEngine.cpp | 5 +---- src/QtLocationPlugin/QGCMapUrlEngine.cpp | 2 +- src/QtLocationPlugin/QMLControl/OfflineMap.qml | 8 +++++--- src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc | 4 ++++ src/TerrainTile.h | 2 -- 5 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapEngine.cpp b/src/QtLocationPlugin/QGCMapEngine.cpp index 65fdcab..abcbcc4 100644 --- a/src/QtLocationPlugin/QGCMapEngine.cpp +++ b/src/QtLocationPlugin/QGCMapEngine.cpp @@ -112,7 +112,7 @@ getQGCMapEngine() } //----------------------------------------------------------------------------- -const double QGCMapEngine::srtm1TileSize = 0.025; +const double QGCMapEngine::srtm1TileSize = 0.01; //----------------------------------------------------------------------------- void @@ -393,9 +393,6 @@ QGCMapEngine::getMapNameList() mapList << kEsriTypes[i].name; } } - for(size_t i = 0; i < NUM_ELEVMAPS; i++) { - mapList << kElevationTypes[i].name; - } return mapList; } diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 95c6de3..74cea6c 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -552,7 +552,7 @@ UrlFactory::_tryCorrectGoogleVersions(QNetworkAccessManager* networkManager) #define AVERAGE_MAPBOX_SAT_MAP 15739 #define AVERAGE_MAPBOX_STREET_MAP 5648 #define AVERAGE_TILE_SIZE 13652 -#define AVERAGE_AIRMAP_ELEV_SIZE 34000 +#define AVERAGE_AIRMAP_ELEV_SIZE 5360 //----------------------------------------------------------------------------- quint32 diff --git a/src/QtLocationPlugin/QMLControl/OfflineMap.qml b/src/QtLocationPlugin/QMLControl/OfflineMap.qml index cd132a5..9fc876e 100644 --- a/src/QtLocationPlugin/QMLControl/OfflineMap.qml +++ b/src/QtLocationPlugin/QMLControl/OfflineMap.qml @@ -433,7 +433,7 @@ QGCView { Row { spacing: ScreenTools.defaultFontPixelWidth anchors.horizontalCenter: parent.horizontalCenter - visible: !_defaultSet + visible: !_defaultSet && mapType !== "Airmap Elevation Data" QGCLabel { text: qsTr("Zoom Levels:"); width: infoView._labelWidth; } QGCLabel { text: offlineMapView._currentSelection ? (offlineMapView._currentSelection.minZoom + " - " + offlineMapView._currentSelection.maxZoom) : ""; horizontalAlignment: Text.AlignRight; width: infoView._valueWidth; } } @@ -737,8 +737,10 @@ QGCView { anchors.right: parent.right text: qsTr("Fetch elevation data") checked: QGroundControl.mapEngineManager.fetchElevation - onClicked: QGroundControl.mapEngineManager.fetchElevation = checked - visible: mapType != "Airmap Elevation Data" + onClicked: { + QGroundControl.mapEngineManager.fetchElevation = checked + handleChanges() + } } } diff --git a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc index ea64716..926845c 100644 --- a/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc +++ b/src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc @@ -83,6 +83,10 @@ QGCMapEngineManager::updateForCurrentView(double lon0, double lat0, double lon1, QGCTileSet set = QGCMapEngine::getTileCount(z, lon0, lat0, lon1, lat1, mapType); _totalSet += set; } + if (_fetchElevation) { + QGCTileSet set = QGCMapEngine::getTileCount(1, lon0, lat0, lon1, lat1, UrlFactory::AirmapElevation); + _totalSet += set; + } emit tileX0Changed(); emit tileX1Changed(); emit tileY0Changed(); diff --git a/src/TerrainTile.h b/src/TerrainTile.h index ccc6ac8..6f6254f 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -5,8 +5,6 @@ #include -#define TERRAIN_TILE_SIZE 91 - Q_DECLARE_LOGGING_CATEGORY(TerrainTileLog) /** From 0f7c1a137984ff45f01aa1a5cb0f0c76ee687831 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 19 Dec 2017 09:54:51 -0500 Subject: [PATCH 16/29] fix --- src/TerrainTile.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index d1b6394..c68e42a 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -36,9 +36,9 @@ TerrainTile::~TerrainTile() if (_data) { for (int i = 0; i < _gridSizeLat; i++) { delete _data[i]; - delete _data; - _data = NULL; } + delete _data; + _data = NULL; } } From ec93ab966f3b00d6465699ec172101be071ec3e4 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 15 Mar 2018 07:44:10 +0100 Subject: [PATCH 17/29] remove artefact --- src/Terrain.cc.orig | 378 ---------------------------------------------------- src/Terrain.h | 10 -- 2 files changed, 388 deletions(-) delete mode 100644 src/Terrain.cc.orig diff --git a/src/Terrain.cc.orig b/src/Terrain.cc.orig deleted file mode 100644 index 27670b0..0000000 --- a/src/Terrain.cc.orig +++ /dev/null @@ -1,378 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -#include "Terrain.h" -#include "QGCMapEngine.h" -#include "QGeoMapReplyQGC.h" - -#include -#include -#include -#include -#include -#include -#include -#include -<<<<<<< HEAD -#include - -QGC_LOGGING_CATEGORY(TerrainLog, "TerrainLog") - -QMutex ElevationProvider::_tilesMutex; -QHash ElevationProvider::_tiles; -QStringList ElevationProvider::_downloadQueue; -======= -#include ->>>>>>> pull_upstream - -QGC_LOGGING_CATEGORY(ElevationProviderLog, "ElevationProviderLog") - -Q_GLOBAL_STATIC(TerrainBatchManager, _terrainBatchManager) - -TerrainBatchManager::TerrainBatchManager(void) -{ - _batchTimer.setSingleShot(true); - _batchTimer.setInterval(_batchTimeout); - connect(&_batchTimer, &QTimer::timeout, this, &TerrainBatchManager::_sendNextBatch); -} - -void TerrainBatchManager::addQuery(ElevationProvider* elevationProvider, const QList& coordinates) -{ - if (coordinates.length() > 0) { - qCDebug(ElevationProviderLog) << "addQuery: elevationProvider:coordinates.count" << elevationProvider << coordinates.count(); - connect(elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - QueuedRequestInfo_t queuedRequestInfo = { elevationProvider, coordinates }; - _requestQueue.append(queuedRequestInfo); - if (!_batchTimer.isActive()) { - _batchTimer.start(); - } - } -} - -<<<<<<< HEAD -bool ElevationProvider::queryTerrainDataPoints(const QList& coordinates) -======= -void TerrainBatchManager::_sendNextBatch(void) ->>>>>>> pull_upstream -{ - qCDebug(ElevationProviderLog) << "_sendNextBatch _state:_requestQueue.count:_sentRequests.count" << _stateToString(_state) << _requestQueue.count() << _sentRequests.count(); - - if (_state != State::Idle) { - // Waiting for last download the complete, wait some more - _batchTimer.start(); - return; - } - - if (_requestQueue.count() == 0) { - return; - } - - _sentRequests.clear(); - - // Convert coordinates to point strings for json query - QString points; - foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { - SentRequestInfo_t sentRequestInfo = { requestInfo.elevationProvider, false, requestInfo.coordinates.count() }; - qCDebug(ElevationProviderLog) << "Building request: coordinate count" << requestInfo.coordinates.count(); - _sentRequests.append(sentRequestInfo); - - foreach (const QGeoCoordinate& coord, requestInfo.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 - _requestQueue.clear(); - - QUrlQuery query; - query.addQueryItem(QStringLiteral("points"), points); - QUrl url(QStringLiteral("https://api.airmap.com/elevation/stage/srtm1/ele")); - url.setQuery(query); - - QNetworkRequest request(url); - - QNetworkProxy tProxy; - tProxy.setType(QNetworkProxy::DefaultProxy); - _networkManager.setProxy(tProxy); - - QNetworkReply* networkReply = _networkManager.get(request); - if (!networkReply) { - _batchFailed(); - return; - } - - connect(networkReply, &QNetworkReply::finished, this, &TerrainBatchManager::_requestFinished); - - _state = State::Downloading; -} - -<<<<<<< HEAD -bool ElevationProvider::queryTerrainData(const QList& coordinates) -{ - if (_state != State::Idle || coordinates.length() == 0) { - return false; - } - - QList altitudes; - bool needToDownload = false; - foreach (QGeoCoordinate coordinate, coordinates) { - QString tileHash = _getTileHash(coordinate); - _tilesMutex.lock(); - if (!_tiles.contains(tileHash)) { - qCDebug(TerrainLog) << "Need to download tile " << tileHash; - - if (!_downloadQueue.contains(tileHash)) { - // Schedule the fetch task - - QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); - QGeoTileSpec spec; - spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); - spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); - spec.setZoom(1); - spec.setMapId(UrlFactory::AirmapElevation); - QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); - connect(reply, &QGeoTiledMapReplyQGC::finished, this, &ElevationProvider::_fetchedTile); - connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &ElevationProvider::_fetchedTile); - - _downloadQueue.append(tileHash); - } - needToDownload = true; - } else { - if (!needToDownload) { - if (_tiles[tileHash].isIn(coordinate)) { - altitudes.push_back(_tiles[tileHash].elevation(coordinate)); - } else { - qCDebug(TerrainLog) << "Error: coordinate not in tile region"; - altitudes.push_back(-1.0); - } - } - } - _tilesMutex.unlock(); - } - - if (!needToDownload) { - qCDebug(TerrainLog) << "All altitudes taken from cached data"; - emit terrainData(true, altitudes); - _coordinates.clear(); - return true; - } - - _coordinates = coordinates; - - return false; -} - -void ElevationProvider::_requestFinished() -======= -void TerrainBatchManager::_batchFailed(void) -{ - QList noAltitudes; - - foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { - if (!sentRequestInfo.providerDestroyed) { - disconnect(sentRequestInfo.elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - sentRequestInfo.elevationProvider->_signalTerrainData(false, noAltitudes); - } - } - _sentRequests.clear(); -} - -void TerrainBatchManager::_requestFinished() ->>>>>>> pull_upstream -{ - QNetworkReply* reply = qobject_cast(QObject::sender()); - - _state = State::Idle; - - // When an error occurs we still end up here - if (reply->error() != QNetworkReply::NoError) { -<<<<<<< HEAD - QByteArray responseBytes = reply->readAll(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - qCDebug(TerrainLog) << "Received " << responseJson; - emit terrainData(false, altitudes); -======= - qCDebug(ElevationProviderLog) << "_requestFinished error:" << reply->error(); - _batchFailed(); - reply->deleteLater(); ->>>>>>> pull_upstream - return; - } - - QByteArray responseBytes = reply->readAll(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCDebug(ElevationProviderLog) << "_requestFinished unable to parse json:" << parseError.errorString(); - _batchFailed(); - reply->deleteLater(); - return; - } - - QJsonObject rootObject = responseJson.object(); - QString status = rootObject["status"].toString(); - if (status != "success") { - qCDebug(ElevationProviderLog) << "_requestFinished status != success:" << status; - _batchFailed(); - reply->deleteLater(); - return; - } - - QList altitudes; - const QJsonArray& dataArray = rootObject["data"].toArray(); - for (int i = 0; i < dataArray.count(); i++) { - altitudes.push_back(dataArray[i].toDouble()); - } - - int currentIndex = 0; - foreach (const SentRequestInfo_t& sentRequestInfo, _sentRequests) { - if (!sentRequestInfo.providerDestroyed) { - disconnect(sentRequestInfo.elevationProvider, &ElevationProvider::destroyed, this, &TerrainBatchManager::_elevationProviderDestroyed); - QList requestAltitudes = altitudes.mid(currentIndex, sentRequestInfo.cCoord); - sentRequestInfo.elevationProvider->_signalTerrainData(true, requestAltitudes); - currentIndex += sentRequestInfo.cCoord; - } - } - _sentRequests.clear(); - - reply->deleteLater(); -} - -void TerrainBatchManager::_elevationProviderDestroyed(QObject* elevationProvider) -{ - // Remove/Mark deleted objects queries from queues - - qCDebug(ElevationProviderLog) << "_elevationProviderDestroyed elevationProvider" << elevationProvider; - - int i = 0; - while (i < _requestQueue.count()) { - const QueuedRequestInfo_t& requestInfo = _requestQueue[i]; - if (requestInfo.elevationProvider == elevationProvider) { - qCDebug(ElevationProviderLog) << "Removing deleted provider from _requestQueue index:elevationProvider" << i << requestInfo.elevationProvider; - _requestQueue.removeAt(i); - } else { - i++; - } - } - - for (int i=0; i<_sentRequests.count(); i++) { - SentRequestInfo_t& sentRequestInfo = _sentRequests[i]; - if (sentRequestInfo.elevationProvider == elevationProvider) { - qCDebug(ElevationProviderLog) << "Zombieing deleted provider from _sentRequests index:elevatationProvider" << sentRequestInfo.elevationProvider; - sentRequestInfo.providerDestroyed = true; - } - } -} - -QString TerrainBatchManager::_stateToString(State state) -{ - switch (state) { - case State::Idle: - return QStringLiteral("Idle"); - case State::Downloading: - return QStringLiteral("Downloading"); - } - - return QStringLiteral("State unknown"); -} - -ElevationProvider::ElevationProvider(QObject* parent) - : QObject(parent) -{ - -} -void ElevationProvider::queryTerrainData(const QList& coordinates) -{ - if (coordinates.length() == 0) { - return; - } - - _terrainBatchManager->addQuery(this, coordinates); -} - -void ElevationProvider::_signalTerrainData(bool success, QList& altitudes) -{ - emit terrainData(success, altitudes); -} - -void ElevationProvider::_fetchedTile() -{ - QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); - - if (!reply) { - qCDebug(TerrainLog) << "Elevation tile fetched but invalid reply data type."; - return; - } - - // remove from download queue - QGeoTileSpec spec = reply->tileSpec(); - QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); - _tilesMutex.lock(); - if (_downloadQueue.contains(hash)) { - _downloadQueue.removeOne(hash); - } - _tilesMutex.unlock(); - - // handle potential errors - if (reply->error() != QGeoTiledMapReply::NoError) { - if (reply->error() == QGeoTiledMapReply::CommunicationError) { - qCDebug(TerrainLog) << "Elevation tile fetching returned communication error. " << reply->errorString(); - } else { - qCDebug(TerrainLog) << "Elevation tile fetching returned error. " << reply->errorString(); - } - reply->deleteLater(); - return; - } - if (!reply->isFinished()) { - qCDebug(TerrainLog) << "Error in fetching elevation tile. Not finished. " << reply->errorString(); - reply->deleteLater(); - return; - } - - // parse received data and insert into hash table - QByteArray responseBytes = reply->mapImageData(); - - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCDebug(TerrainLog) << "Could not parse terrain tile " << parseError.errorString(); - qCDebug(TerrainLog) << responseBytes; - reply->deleteLater(); - return; - } - - TerrainTile* terrainTile = new TerrainTile(responseJson); - if (terrainTile->isValid()) { - _tilesMutex.lock(); - if (!_tiles.contains(hash)) { - _tiles.insert(hash, *terrainTile); - } else { - delete terrainTile; - } - _tilesMutex.unlock(); - } else { - qCDebug(TerrainLog) << "Received invalid tile"; - } - reply->deleteLater(); - - // now try to query the data again - queryTerrainData(_coordinates); -} - -QString ElevationProvider::_getTileHash(const QGeoCoordinate& coordinate) -{ - QString ret = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1); - qCDebug(TerrainLog) << "Computing unique tile hash for " << coordinate << ret; - - return ret; -} diff --git a/src/Terrain.h b/src/Terrain.h index 922047a..f6ec9d5 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -20,16 +20,6 @@ #include #include -/* usage example: - ElevationProvider *p = new ElevationProvider(); - QList coordinates; - QGeoCoordinate c(47.379243, 8.548265); - coordinates.push_back(c); - c.setLatitude(c.latitude()+0.01); - coordinates.push_back(c); - p->queryTerrainData(coordinates); - */ - Q_DECLARE_LOGGING_CATEGORY(ElevationProviderLog) class ElevationProvider; From 6f474c991d36279c095b18890f43a77a616f11e9 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 15 Mar 2018 08:16:04 +0100 Subject: [PATCH 18/29] compile fix --- src/Terrain.cc | 13 +++++++++++-- src/Terrain.h | 1 - 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/Terrain.cc b/src/Terrain.cc index 8a30ef6..4533424 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -41,7 +41,7 @@ void TerrainBatchManager::addQuery(ElevationProvider* elevationProvider, const Q } qCDebug(ElevationProviderLog) << "All altitudes taken from cached data"; - elevationProvider->_signalTerrainData(true, altitudes); + elevationProvider->_signalTerrainData(coordinates.count() == altitudes.count(), altitudes); } } @@ -71,6 +71,7 @@ bool TerrainBatchManager::_getAltitudesForCoordinates(const QListerrorString(); } + _tileFailed(); reply->deleteLater(); return; } if (!reply->isFinished()) { qCDebug(ElevationProviderLog) << "Error in fetching elevation tile. Not finished. " << reply->errorString(); + _tileFailed(); reply->deleteLater(); return; } @@ -140,6 +143,7 @@ void TerrainBatchManager::_fetchedTile() if (parseError.error != QJsonParseError::NoError) { qCDebug(ElevationProviderLog) << "Could not parse terrain tile " << parseError.errorString(); qCDebug(ElevationProviderLog) << responseBytes; + _tileFailed(); reply->deleteLater(); return; } @@ -162,7 +166,7 @@ void TerrainBatchManager::_fetchedTile() for (int i = _requestQueue.count() - 1; i >= 0; i--) { QList altitudes; if (_getAltitudesForCoordinates(_requestQueue[i].coordinates, altitudes)) { - _requestQueue[i].elevationProvider->_signalTerrainData(true, altitudes); + _requestQueue[i].elevationProvider->_signalTerrainData(_requestQueue[i].coordinates.count() == altitudes.count(), altitudes); _requestQueue.removeAt(i); } } @@ -192,3 +196,8 @@ bool ElevationProvider::queryTerrainData(const QList& coordinate return false; } + +void ElevationProvider::_signalTerrainData(bool success, QList& altitudes) +{ + emit terrainData(success, altitudes); +} diff --git a/src/Terrain.h b/src/Terrain.h index f6ec9d5..36f642c 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -34,7 +34,6 @@ public: void addQuery(ElevationProvider* elevationProvider, const QList& coordinates); private slots: - void _requestFinished (void); void _fetchedTile (void); /// slot to handle fetched elevation tiles private: From c409543009c1cb9453575e3439717baeb2b944b3 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 16 Mar 2018 08:55:21 +0100 Subject: [PATCH 19/29] fix airmap api --- src/QtLocationPlugin/QGCMapUrlEngine.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 0785e0a..6c20920 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -415,10 +415,10 @@ UrlFactory::_getURL(MapType type, int x, int y, int zoom, QNetworkAccessManager* break; case AirmapElevation: { - return QString("https://api.airmap.com/elevation/stage/srtm1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( - static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( - static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0).arg( - static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0); + return QString("https://api.airmap.com/elevation/v1/ele/carpet?points=%1,%2,%3,%4").arg(static_cast(y)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x)*QGCMapEngine::srtm1TileSize - 180.0).arg( + static_cast(y + 1)*QGCMapEngine::srtm1TileSize - 90.0).arg( + static_cast(x + 1)*QGCMapEngine::srtm1TileSize - 180.0); } break; From 96fc6a8cf9ea31ac4d55fd2d10f7d1d4376d5170 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 16 Mar 2018 09:23:14 +0100 Subject: [PATCH 20/29] download fix --- src/Terrain.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Terrain.cc b/src/Terrain.cc index 4533424..38560f0 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -38,6 +38,7 @@ void TerrainBatchManager::addQuery(ElevationProvider* elevationProvider, const Q if (!_getAltitudesForCoordinates(coordinates, altitudes)) { QueuedRequestInfo_t queuedRequestInfo = { elevationProvider, coordinates }; _requestQueue.append(queuedRequestInfo); + return; } qCDebug(ElevationProviderLog) << "All altitudes taken from cached data"; @@ -100,6 +101,7 @@ void TerrainBatchManager::_tileFailed(void) void TerrainBatchManager::_fetchedTile() { QGeoTiledMapReplyQGC* reply = qobject_cast(QObject::sender()); + _state = State::Idle; if (!reply) { qCDebug(ElevationProviderLog) << "Elevation tile fetched but invalid reply data type."; From 32a35ac910c13e65c1959a3d453a19cecfb08f4e Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Fri, 16 Mar 2018 11:46:03 +0100 Subject: [PATCH 21/29] remove tile download queue --- src/Terrain.cc | 36 ++++++++++++------------------------ src/Terrain.h | 1 - 2 files changed, 12 insertions(+), 25 deletions(-) diff --git a/src/Terrain.cc b/src/Terrain.cc index 38560f0..55dd815 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -54,23 +54,18 @@ bool TerrainBatchManager::_getAltitudesForCoordinates(const QListurlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); - QGeoTileSpec spec; - spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); - spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); - spec.setZoom(1); - spec.setMapId(UrlFactory::AirmapElevation); - QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); - connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainBatchManager::_fetchedTile); - connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &TerrainBatchManager::_fetchedTile); - _state = State::Downloading; - } - - _tileDownloadQueue.append(tileHash); + // Schedule the fetch task + if (_state != State::Downloading) { + QNetworkRequest request = getQGCMapEngine()->urlFactory()->getTileURL(UrlFactory::AirmapElevation, QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1), QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1), 1, &_networkManager); + QGeoTileSpec spec; + spec.setX(QGCMapEngine::long2elevationTileX(coordinate.longitude(), 1)); + spec.setY(QGCMapEngine::lat2elevationTileY(coordinate.latitude(), 1)); + spec.setZoom(1); + spec.setMapId(UrlFactory::AirmapElevation); + QGeoTiledMapReplyQGC* reply = new QGeoTiledMapReplyQGC(&_networkManager, request, spec); + connect(reply, &QGeoTiledMapReplyQGC::finished, this, &TerrainBatchManager::_fetchedTile); + connect(reply, &QGeoTiledMapReplyQGC::aborted, this, &TerrainBatchManager::_fetchedTile); + _state = State::Downloading; } _tilesMutex.unlock(); @@ -111,13 +106,6 @@ void TerrainBatchManager::_fetchedTile() // remove from download queue QGeoTileSpec spec = reply->tileSpec(); QString hash = QGCMapEngine::getTileHash(UrlFactory::AirmapElevation, spec.x(), spec.y(), spec.zoom()); - _tilesMutex.lock(); - if (_tileDownloadQueue.contains(hash)) { - _tileDownloadQueue.removeOne(hash); - } else { - qCDebug(ElevationProviderLog) << "Loaded elevation tile, but not found in download queue."; - } - _tilesMutex.unlock(); // handle potential errors if (reply->error() != QGeoTiledMapReply::NoError) { diff --git a/src/Terrain.h b/src/Terrain.h index 36f642c..18bc55f 100644 --- a/src/Terrain.h +++ b/src/Terrain.h @@ -57,7 +57,6 @@ private: QMutex _tilesMutex; QHash _tiles; - QStringList _tileDownloadQueue; }; class ElevationProvider : public QObject From 612d219decf5d4213d4747bf1e89c1c703139540 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Fri, 16 Mar 2018 17:52:24 +0100 Subject: [PATCH 22/29] first implementation of binary representation of elevation data --- src/QtLocationPlugin/QGeoMapReplyQGC.cpp | 22 ++++- src/Terrain.cc | 12 +-- src/TerrainTile.cc | 161 +++++++++++++++++++------------ src/TerrainTile.h | 22 ++++- 4 files changed, 141 insertions(+), 76 deletions(-) diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index d95f1db..b07ad39 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -51,6 +51,10 @@ #include #include #include +#include "TerrainTile.h" +#include +#include +#include int QGeoTiledMapReplyQGC::_requestCount = 0; @@ -121,8 +125,24 @@ QGeoTiledMapReplyQGC::networkReplyFinished() return; } QByteArray a = _reply->readAll(); - setMapImageData(a); QString format = getQGCMapEngine()->urlFactory()->getImageFormat((UrlFactory::MapType)tileSpec().mapId(), a); + + // convert "a" to binary in case we have elevation data + if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { + QJsonParseError parseError; + QJsonDocument json = QJsonDocument::fromJson(a, &parseError); + if (parseError.error != QJsonParseError::NoError) { + emit aborted(); + return; + } else { + a = TerrainTile::serialize(json); + if (a.isEmpty()) { + emit aborted(); + return; + } + } + } + setMapImageData(a); if(!format.isEmpty()) { setMapImageFormat(format); getQGCMapEngine()->cacheTile((UrlFactory::MapType)tileSpec().mapId(), tileSpec().x(), tileSpec().y(), tileSpec().zoom(), a, format); diff --git a/src/Terrain.cc b/src/Terrain.cc index 55dd815..4080eb2 100644 --- a/src/Terrain.cc +++ b/src/Terrain.cc @@ -128,17 +128,7 @@ void TerrainBatchManager::_fetchedTile() // parse received data and insert into hash table QByteArray responseBytes = reply->mapImageData(); - QJsonParseError parseError; - QJsonDocument responseJson = QJsonDocument::fromJson(responseBytes, &parseError); - if (parseError.error != QJsonParseError::NoError) { - qCDebug(ElevationProviderLog) << "Could not parse terrain tile " << parseError.errorString(); - qCDebug(ElevationProviderLog) << responseBytes; - _tileFailed(); - reply->deleteLater(); - return; - } - - TerrainTile* terrainTile = new TerrainTile(responseJson); + TerrainTile* terrainTile = new TerrainTile(responseBytes); if (terrainTile->isValid()) { _tilesMutex.lock(); if (!_tiles.contains(hash)) { diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index c68e42a..e1f772f 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -5,6 +5,7 @@ #include #include #include +#include QGC_LOGGING_CATEGORY(TerrainTileLog, "TerrainTileLog") @@ -42,15 +43,83 @@ TerrainTile::~TerrainTile() } } -TerrainTile::TerrainTile(QJsonDocument document) + +TerrainTile::TerrainTile(QByteArray byteArray) : _minElevation(-1.0) , _maxElevation(-1.0) , _avgElevation(-1.0) + , _data(NULL) + , _gridSizeLat(-1) + , _gridSizeLon(-1) , _isValid(false) { + QDataStream stream(byteArray); + + stream >> _southWest + >> _northEast + >> _minElevation + >> _maxElevation + >> _avgElevation + >> _gridSizeLat + >> _gridSizeLon; + + for (int i = 0; i < _gridSizeLat; i++) { + if (i == 0) { + _data = new double*[_gridSizeLat]; + for (int k = 0; k < _gridSizeLat; k++) { + _data[k] = new double[_gridSizeLon]; + } + } + for (int j = 0; j < _gridSizeLon; j++) { + stream >> _data[i][j]; + } + } + + _isValid = true; +} + + +bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const +{ + if (!_isValid) { + qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; + return false; + } + bool ret = coordinate.latitude() >= _southWest.latitude() && coordinate.longitude() >= _southWest.longitude() && + coordinate.latitude() <= _northEast.latitude() && coordinate.longitude() <= _northEast.longitude(); + qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; + return ret; +} + +float TerrainTile::elevation(const QGeoCoordinate& coordinate) const +{ + if (_isValid) { + qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; + // Get the index at resolution of 1 arc second + int indexLat = _latToDataIndex(coordinate.latitude()); + int indexLon = _lonToDataIndex(coordinate.longitude()); + qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; + return _data[indexLat][indexLon]; + } else { + qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; + return -1.0; + } +} + +QGeoCoordinate TerrainTile::centerCoordinate(void) const +{ + return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); +} + +QByteArray TerrainTile::serialize(QJsonDocument document) +{ + QByteArray byteArray; + QIODevice::OpenMode writeonly = QIODevice::WriteOnly; + QDataStream stream(&byteArray, writeonly); if (!document.isObject()) { qCDebug(TerrainTileLog) << "Terrain tile json doc is no object"; - return; + QByteArray emptyArray; + return emptyArray; } QJsonObject rootObject = document.object(); @@ -61,12 +130,14 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return; + QByteArray emptyArray; + return emptyArray; } if (rootObject[_jsonStatusKey].toString() != "success") { qCDebug(TerrainTileLog) << "Invalid terrain tile."; - return; + QByteArray emptyArray; + return emptyArray; } const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject(); QList dataVersionKeyInfoList = { @@ -76,7 +147,8 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return; + QByteArray emptyArray; + return emptyArray; } // Bounds @@ -87,18 +159,18 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(boundsObject, boundsVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return; + QByteArray emptyArray; + return emptyArray; } const QJsonArray& swArray = boundsObject[_jsonSouthWestKey].toArray(); const QJsonArray& neArray = boundsObject[_jsonNorthEastKey].toArray(); if (swArray.count() < 2 || neArray.count() < 2 ) { qCDebug(TerrainTileLog) << "Incomplete bounding location"; - return; + QByteArray emptyArray; + return emptyArray; } - _southWest.setLatitude(swArray[0].toDouble()); - _southWest.setLongitude(swArray[1].toDouble()); - _northEast.setLatitude(neArray[0].toDouble()); - _northEast.setLongitude(neArray[1].toDouble()); + stream << QGeoCoordinate(swArray[0].toDouble(), swArray[1].toDouble()); + stream << QGeoCoordinate(neArray[0].toDouble(), neArray[1].toDouble()); // Stats const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); @@ -109,70 +181,39 @@ TerrainTile::TerrainTile(QJsonDocument document) }; if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; - return; + QByteArray emptyArray; + return emptyArray; } - _maxElevation = statsObject[_jsonMaxElevationKey].toInt(); - _minElevation = statsObject[_jsonMinElevationKey].toInt(); - _avgElevation = statsObject[_jsonAvgElevationKey].toInt(); + stream << statsObject[_jsonMaxElevationKey].toInt(); + stream << statsObject[_jsonMinElevationKey].toInt(); + stream << statsObject[_jsonAvgElevationKey].toDouble(); // Carpet const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); - _gridSizeLat = carpetArray.count(); + int gridSizeLat = carpetArray.count(); + stream << gridSizeLat; + int gridSizeLon = 0; qCDebug(TerrainTileLog) << "Received tile has size in latitude direction: " << carpetArray.count(); - for (int i = 0; i < _gridSizeLat; i++) { + for (int i = 0; i < gridSizeLat; i++) { const QJsonArray& row = carpetArray[i].toArray(); if (i == 0) { - _gridSizeLon = row.count(); + gridSizeLon = row.count(); + stream << gridSizeLon; qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << row.count(); - if (_gridSizeLon > 0) { - _data = new float*[_gridSizeLat]; - } - for (int k = 0; k < _gridSizeLat; k++) { - _data[k] = new float[_gridSizeLon]; - } } - if (row.count() < _gridSizeLon) { - qCDebug(TerrainTileLog) << "Expected row array of " << _gridSizeLon << ", instead got " << row.count(); - return; + if (row.count() < gridSizeLon) { + qCDebug(TerrainTileLog) << "Expected row array of " << gridSizeLon << ", instead got " << row.count(); + QByteArray emptyArray; + return emptyArray; } - for (int j = 0; j < _gridSizeLon; j++) { - _data[i][j] = row[j].toDouble(); + for (int j = 0; j < gridSizeLon; j++) { + stream << row[j].toDouble(); } } - _isValid = true; -} -bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const -{ - if (!_isValid) { - qCDebug(TerrainTileLog) << "isIn requested, but tile not valid"; - return false; - } - bool ret = coordinate.latitude() >= _southWest.latitude() && coordinate.longitude() >= _southWest.longitude() && - coordinate.latitude() <= _northEast.latitude() && coordinate.longitude() <= _northEast.longitude(); - qCDebug(TerrainTileLog) << "Checking isIn: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast << ": " << ret; - return ret; + return byteArray; } -float TerrainTile::elevation(const QGeoCoordinate& coordinate) const -{ - if (_isValid) { - qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; - // Get the index at resolution of 1 arc second - int indexLat = _latToDataIndex(coordinate.latitude()); - int indexLon = _lonToDataIndex(coordinate.longitude()); - qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; - return _data[indexLat][indexLon]; - } else { - qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; - return -1.0; - } -} - -QGeoCoordinate TerrainTile::centerCoordinate(void) const -{ - return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); -} int TerrainTile::_latToDataIndex(double latitude) const { diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 6f6254f..84de079 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -27,6 +27,13 @@ public: TerrainTile(QJsonDocument document); /** + * Constructor from serialized elevation data (either from file or web) + * + * @param document + */ + TerrainTile(QByteArray byteArray); + + /** * Check for whether a coordinate lies within this tile * * @param coordinate @@ -77,6 +84,13 @@ public: */ QGeoCoordinate centerCoordinate(void) const; + /** + * Serialize data + * + * @return serialized data + */ + static QByteArray serialize(QJsonDocument document); + private: inline int _latToDataIndex(double latitude) const; inline int _lonToDataIndex(double longitude) const; @@ -84,11 +98,11 @@ private: QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile - float _minElevation; /// Minimum elevation in tile - float _maxElevation; /// Maximum elevation in tile - float _avgElevation; /// Average elevation of the tile + int _minElevation; /// Minimum elevation in tile + int _maxElevation; /// Maximum elevation in tile + double _avgElevation; /// Average elevation of the tile - float** _data; /// 2D elevation data array + double** _data; /// 2D elevation data array int _gridSizeLat; /// data grid size in latitude direction int _gridSizeLon; /// data grid size in longitude direction bool _isValid; /// data loaded is valid From 7b00e7a32f6cecafa0180209a7d3bff4abdae0ca Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Mon, 19 Mar 2018 16:20:32 +0100 Subject: [PATCH 23/29] first implementation of binary representation of elevation data --- src/QtLocationPlugin/QGeoMapReplyQGC.cpp | 16 ++++------------ src/TerrainTile.cc | 12 +++++++++--- src/TerrainTile.h | 10 +++++----- 3 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp index b07ad39..44ed968 100644 --- a/src/QtLocationPlugin/QGeoMapReplyQGC.cpp +++ b/src/QtLocationPlugin/QGeoMapReplyQGC.cpp @@ -52,9 +52,6 @@ #include #include #include "TerrainTile.h" -#include -#include -#include int QGeoTiledMapReplyQGC::_requestCount = 0; @@ -129,18 +126,13 @@ QGeoTiledMapReplyQGC::networkReplyFinished() // convert "a" to binary in case we have elevation data if ((UrlFactory::MapType)tileSpec().mapId() == UrlFactory::MapType::AirmapElevation) { - QJsonParseError parseError; - QJsonDocument json = QJsonDocument::fromJson(a, &parseError); - if (parseError.error != QJsonParseError::NoError) { + + a = TerrainTile::serialize(a); + if (a.isEmpty()) { emit aborted(); return; - } else { - a = TerrainTile::serialize(json); - if (a.isEmpty()) { - emit aborted(); - return; - } } + } setMapImageData(a); if(!format.isEmpty()) { diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index e1f772f..2a065e6 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -111,11 +111,17 @@ QGeoCoordinate TerrainTile::centerCoordinate(void) const return _southWest.atDistanceAndAzimuth(_southWest.distanceTo(_northEast) / 2.0, _southWest.azimuthTo(_northEast)); } -QByteArray TerrainTile::serialize(QJsonDocument document) +QByteArray TerrainTile::serialize(QByteArray input) { + QJsonParseError parseError; + QJsonDocument document = QJsonDocument::fromJson(input, &parseError); + if (parseError.error != QJsonParseError::NoError) { + QByteArray emptyArray; + return emptyArray; + } + QByteArray byteArray; - QIODevice::OpenMode writeonly = QIODevice::WriteOnly; - QDataStream stream(&byteArray, writeonly); + QDataStream stream(&byteArray, QIODevice::WriteOnly); if (!document.isObject()) { qCDebug(TerrainTileLog) << "Terrain tile json doc is no object"; QByteArray emptyArray; diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 84de079..faee055 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -89,7 +89,7 @@ public: * * @return serialized data */ - static QByteArray serialize(QJsonDocument document); + static QByteArray serialize(QByteArray input); private: inline int _latToDataIndex(double latitude) const; @@ -98,11 +98,11 @@ private: QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile - int _minElevation; /// Minimum elevation in tile - int _maxElevation; /// Maximum elevation in tile - double _avgElevation; /// Average elevation of the tile + int _minElevation; /// Minimum elevation in tile + int _maxElevation; /// Maximum elevation in tile + double _avgElevation; /// Average elevation of the tile - double** _data; /// 2D elevation data array + double** _data; /// 2D elevation data array int _gridSizeLat; /// data grid size in latitude direction int _gridSizeLon; /// data grid size in longitude direction bool _isValid; /// data loaded is valid From f01cd04cf37b4a166b0fcd632d6e347ec180b18a Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Tue, 20 Mar 2018 08:49:45 +0100 Subject: [PATCH 24/29] fixes --- src/Terrain/TerrainQuery.cc | 20 +++++++++----------- src/Terrain/TerrainQuery.h | 16 ++++++++-------- src/TerrainTile.cc | 6 +++--- src/TerrainTile.h | 16 ++++++++-------- 4 files changed, 28 insertions(+), 30 deletions(-) diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 829718a..3c63063 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -229,12 +229,10 @@ TerrainOfflineAirMapQuery::TerrainOfflineAirMapQuery(QObject* parent) void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList& coordinates) { if (coordinates.length() == 0) { - return false; + return; } - _tileBatchManager->addQuery(this, coordinates); - - return false; + _terrainTileManager->addCoordinateQuery(this, coordinates); } void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) @@ -249,7 +247,7 @@ void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoo void TerrainOfflineAirMapQuery::_signalCoordinateHeights(bool success, QList heights) { - emit coordinateHeights(success, heights) + emit coordinateHeights(success, heights); } void TerrainOfflineAirMapQuery::_signalPathHeights(bool success, double latStep, double lonStep, const QList& heights) @@ -267,10 +265,10 @@ TerrainTileManager::TerrainTileManager(void) } -void TerrainTileManager::addQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates) +void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates) { if (coordinates.length() > 0) { - QList altitudes; + QList altitudes; if (!_getAltitudesForCoordinates(coordinates, altitudes)) { QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, coordinates, QueryMode::QueryModeCoordinates }; @@ -279,11 +277,11 @@ void TerrainTileManager::addQuery(TerrainOfflineAirMapQuery* terrainQueryInterfa } qCDebug(TerrainQueryLog) << "All altitudes taken from cached data"; - terrainQueryInterface->_signalTerrainData(coordinates.count() == altitudes.count(), altitudes); + terrainQueryInterface->_signalCoordinateHeights(coordinates.count() == altitudes.count(), altitudes); } } -bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) +bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) { foreach (const QGeoCoordinate& coordinate, coordinates) { QString tileHash = _getTileHash(coordinate); @@ -322,7 +320,7 @@ bool TerrainTileManager::_getAltitudesForCoordinates(const QList void TerrainTileManager::_tileFailed(void) { - QList noAltitudes; + QList noAltitudes; foreach (const QueuedRequestInfo_t& requestInfo, _requestQueue) { if (requestInfo.queryMode == QueryMode::QueryModeCoordinates) { @@ -393,7 +391,7 @@ void TerrainTileManager::_fetchedTile() // now try to query the data again for (int i = _requestQueue.count() - 1; i >= 0; i--) { - QList altitudes; + QList altitudes; if (_getAltitudesForCoordinates(_requestQueue[i].coordinates, altitudes)) { if (_requestQueue[i].queryMode == QueryMode::QueryModeCoordinates) { _requestQueue[i].terrainQueryInterface->_signalCoordinateHeights(_requestQueue[i].coordinates.count() == altitudes.count(), altitudes); diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h index 31cc01d..d79910d 100644 --- a/src/Terrain/TerrainQuery.h +++ b/src/Terrain/TerrainQuery.h @@ -114,18 +114,12 @@ class TerrainTileManager : public QObject { public: TerrainTileManager(void); - void addQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + void addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); private slots: void _fetchedTile (void); /// slot to handle fetched elevation tiles private: - typedef struct { - TerrainOfflineAirMapQuery* terrainQueryInterface; - QList coordinates; - QueryMode queryMode; - } QueuedRequestInfo_t; - enum class State { Idle, Downloading, @@ -137,8 +131,14 @@ private: QueryModeCarpet }; + typedef struct { + TerrainOfflineAirMapQuery* terrainQueryInterface; + QList coordinates; + QueryMode queryMode; + } QueuedRequestInfo_t; + void _tileFailed(void); - bool _getAltitudesForCoordinates(const QList& coordinates, QList& altitudes); + bool _getAltitudesForCoordinates(const QList& coordinates, QList& altitudes); QString _getTileHash(const QGeoCoordinate& coordinate); /// Method to create a unique string for each tile QList _requestQueue; diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index c68e42a..a22fdb5 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -125,10 +125,10 @@ TerrainTile::TerrainTile(QJsonDocument document) _gridSizeLon = row.count(); qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << row.count(); if (_gridSizeLon > 0) { - _data = new float*[_gridSizeLat]; + _data = new double*[_gridSizeLat]; } for (int k = 0; k < _gridSizeLat; k++) { - _data[k] = new float[_gridSizeLon]; + _data[k] = new double[_gridSizeLon]; } } if (row.count() < _gridSizeLon) { @@ -154,7 +154,7 @@ bool TerrainTile::isIn(const QGeoCoordinate& coordinate) const return ret; } -float TerrainTile::elevation(const QGeoCoordinate& coordinate) const +double TerrainTile::elevation(const QGeoCoordinate& coordinate) const { if (_isValid) { qCDebug(TerrainTileLog) << "elevation: " << coordinate << " , in sw " << _southWest << " , ne " << _northEast; diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 6f6254f..200a24d 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -47,28 +47,28 @@ public: * @param coordinate * @return elevation */ - float elevation(const QGeoCoordinate& coordinate) const; + double elevation(const QGeoCoordinate& coordinate) const; /** * Accessor for the minimum elevation of the tile * * @return minimum elevation */ - float minElevation(void) const { return _minElevation; } + double minElevation(void) const { return _minElevation; } /** * Accessor for the maximum elevation of the tile * * @return maximum elevation */ - float maxElevation(void) const { return _maxElevation; } + double maxElevation(void) const { return _maxElevation; } /** * Accessor for the average elevation of the tile * * @return average elevation */ - float avgElevation(void) const { return _avgElevation; } + double avgElevation(void) const { return _avgElevation; } /** * Accessor for the center coordinate @@ -84,11 +84,11 @@ private: QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile - float _minElevation; /// Minimum elevation in tile - float _maxElevation; /// Maximum elevation in tile - float _avgElevation; /// Average elevation of the tile + double _minElevation; /// Minimum elevation in tile + double _maxElevation; /// Maximum elevation in tile + double _avgElevation; /// Average elevation of the tile - float** _data; /// 2D elevation data array + double** _data; /// 2D elevation data array int _gridSizeLat; /// data grid size in latitude direction int _gridSizeLon; /// data grid size in longitude direction bool _isValid; /// data loaded is valid From bdbe526f2e391bb5ac01206a7cbd938361b3d27a Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Tue, 20 Mar 2018 15:53:09 +0100 Subject: [PATCH 25/29] change according to review --- src/TerrainTile.cc | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 2a065e6..87445d3 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -55,9 +55,18 @@ TerrainTile::TerrainTile(QByteArray byteArray) { QDataStream stream(byteArray); - stream >> _southWest - >> _northEast - >> _minElevation + double lat,lon; + stream >> lat + >> lon; + _southWest.setLatitude(lat); + _southWest.setLongitude(lon); + stream >> lat + >> lon; + _northEast.setLatitude(lat); + _northEast.setLongitude(lon); + + + stream >> _minElevation >> _maxElevation >> _avgElevation >> _gridSizeLat @@ -175,8 +184,10 @@ QByteArray TerrainTile::serialize(QByteArray input) QByteArray emptyArray; return emptyArray; } - stream << QGeoCoordinate(swArray[0].toDouble(), swArray[1].toDouble()); - stream << QGeoCoordinate(neArray[0].toDouble(), neArray[1].toDouble()); + stream << swArray[0].toDouble(); + stream << swArray[1].toDouble(); + stream << neArray[0].toDouble(); + stream << neArray[1].toDouble(); // Stats const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); From ac9fbc8aa2ce0ff892627c228a4972ce9f33c4f3 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 21 Mar 2018 09:37:23 +0100 Subject: [PATCH 26/29] remove deadlock and enable path queries --- src/Terrain/TerrainQuery.cc | 43 ++++++++++++++++++++++++++++++++++++++----- src/Terrain/TerrainQuery.h | 5 +++-- src/TerrainTile.h | 3 +++ 3 files changed, 44 insertions(+), 7 deletions(-) diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 3c63063..bcb70a7 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -237,12 +237,16 @@ void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QListaddPathQuery(this, fromCoord, toCoord); } void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) { // TODO + Q_UNUSED(swCoord); + Q_UNUSED(neCoord); + Q_UNUSED(statsOnly); + qWarning() << "Carpet queries are currently not supported from offline air map data"; } void TerrainOfflineAirMapQuery::_signalCoordinateHeights(bool success, QList heights) @@ -252,12 +256,12 @@ void TerrainOfflineAirMapQuery::_signalCoordinateHeights(bool success, QList& heights) { - // TODO + emit pathHeights(success, latStep, lonStep, heights); } void TerrainOfflineAirMapQuery::_signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet) { - // TODO + emit carpetHeights(success, minHeight, maxHeight, carpet); } TerrainTileManager::TerrainTileManager(void) @@ -271,7 +275,7 @@ void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQu QList altitudes; if (!_getAltitudesForCoordinates(coordinates, altitudes)) { - QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, coordinates, QueryMode::QueryModeCoordinates }; + QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModeCoordinates, coordinates }; _requestQueue.append(queuedRequestInfo); return; } @@ -281,6 +285,35 @@ void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQu } } +void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate &startPoint, const QGeoCoordinate &endPoint) +{ + QList coordinates; + double lat = startPoint.latitude(); + double lon = startPoint.longitude(); + double latDiff = endPoint.latitude() - lat; + double lonDiff = endPoint.longitude() - lon; + double steps = ceil(endPoint.distanceTo(startPoint) / TerrainTile::terrainAltitudeSpacing); + for (double i = 0.0; i <= steps; i = i + 1) { + coordinates.append(QGeoCoordinate(lat + latDiff * i / steps, lon + lonDiff * i / steps)); + } + + QList altitudes; + if (!_getAltitudesForCoordinates(coordinates, altitudes)) { + QueuedRequestInfo_t queuedRequestInfo = { terrainQueryInterface, QueryMode::QueryModePath, coordinates }; + _requestQueue.append(queuedRequestInfo); + return; + } + + qCDebug(TerrainQueryLog) << "All altitudes taken from cached data"; + double stepLat = 0; + double stepLon = 0; + if (coordinates.count() > 1) { + stepLat = coordinates[1].latitude() - coordinates[0].latitude(); + stepLon = coordinates[1].longitude() - coordinates[0].longitude(); + } + terrainQueryInterface->_signalPathHeights(coordinates.count() == altitudes.count(), stepLat, stepLon, altitudes); +} + bool TerrainTileManager::_getAltitudesForCoordinates(const QList& coordinates, QList& altitudes) { foreach (const QGeoCoordinate& coordinate, coordinates) { @@ -457,8 +490,8 @@ void TerrainAtCoordinateBatchManager::_sendNextBatch(void) } _requestQueue.clear(); - _terrainQuery.requestCoordinateHeights(coords); _state = State::Downloading; + _terrainQuery.requestCoordinateHeights(coords); } void TerrainAtCoordinateBatchManager::_batchFailed(void) diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h index d79910d..96d98e2 100644 --- a/src/Terrain/TerrainQuery.h +++ b/src/Terrain/TerrainQuery.h @@ -114,7 +114,8 @@ class TerrainTileManager : public QObject { public: TerrainTileManager(void); - void addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + void addCoordinateQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QList& coordinates); + void addPathQuery (TerrainOfflineAirMapQuery* terrainQueryInterface, const QGeoCoordinate& startPoint, const QGeoCoordinate& endPoint); private slots: void _fetchedTile (void); /// slot to handle fetched elevation tiles @@ -133,8 +134,8 @@ private: typedef struct { TerrainOfflineAirMapQuery* terrainQueryInterface; - QList coordinates; QueryMode queryMode; + QList coordinates; } QueuedRequestInfo_t; void _tileFailed(void); diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 200a24d..f8c8c9e 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -77,6 +77,9 @@ public: */ QGeoCoordinate centerCoordinate(void) const; + /// Approximate spacing of the elevation data measurement points + static constexpr double terrainAltitudeSpacing = 30.0; + private: inline int _latToDataIndex(double latitude) const; inline int _lonToDataIndex(double longitude) const; From 6bb01ff3224f8ef2cc9e520782d0e569e6c6b1f3 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 21 Mar 2018 13:39:35 +0100 Subject: [PATCH 27/29] make offline caching working serialized as well --- src/QtLocationPlugin/QGCMapTileSet.cpp | 4 +++ src/QtLocationPlugin/QGCMapUrlEngine.cpp | 7 ++--- src/Terrain/TerrainQuery.cc | 2 ++ src/Terrain/TerrainQuery.h | 6 ++-- src/TerrainTile.cc | 53 ++++++++++++++++++-------------- src/TerrainTile.h | 12 ++++---- 6 files changed, 46 insertions(+), 38 deletions(-) diff --git a/src/QtLocationPlugin/QGCMapTileSet.cpp b/src/QtLocationPlugin/QGCMapTileSet.cpp index 70e70f2..f2ac50f 100644 --- a/src/QtLocationPlugin/QGCMapTileSet.cpp +++ b/src/QtLocationPlugin/QGCMapTileSet.cpp @@ -19,6 +19,7 @@ #include "QGCMapEngine.h" #include "QGCMapTileSet.h" #include "QGCMapEngineManager.h" +#include "TerrainTile.h" #include #include @@ -282,6 +283,9 @@ QGCCachedTileSet::_networkReplyFinished() qCDebug(QGCCachedTileSetLog) << "Tile fetched" << hash; QByteArray image = reply->readAll(); UrlFactory::MapType type = getQGCMapEngine()->hashToType(hash); + if (type == UrlFactory::MapType::AirmapElevation) { + image = TerrainTile::serialize(image); + } QString format = getQGCMapEngine()->urlFactory()->getImageFormat(type, image); if(!format.isEmpty()) { //-- Cache tile diff --git a/src/QtLocationPlugin/QGCMapUrlEngine.cpp b/src/QtLocationPlugin/QGCMapUrlEngine.cpp index 6c20920..ed57ec5 100644 --- a/src/QtLocationPlugin/QGCMapUrlEngine.cpp +++ b/src/QtLocationPlugin/QGCMapUrlEngine.cpp @@ -36,7 +36,6 @@ static const unsigned char pngSignature[] = {0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A, 0x00}; static const unsigned char jpegSignature[] = {0xFF, 0xD8, 0xFF, 0x00}; static const unsigned char gifSignature[] = {0x47, 0x49, 0x46, 0x38, 0x00}; -static const unsigned char jsonSignature[] = {0x7B, 0x22, 0x00}; // two characters '{"' //----------------------------------------------------------------------------- UrlFactory::UrlFactory() @@ -86,8 +85,6 @@ UrlFactory::getImageFormat(MapType type, const QByteArray& image) format = "jpg"; else if (image.startsWith(reinterpret_cast(gifSignature))) format = "gif"; - else if (image.startsWith(reinterpret_cast(jsonSignature))) - format = "json"; else { switch (type) { case GoogleMap: @@ -126,7 +123,7 @@ UrlFactory::getImageFormat(MapType type, const QByteArray& image) format = "jpg"; break; case AirmapElevation: - format = "json"; + format = "bin"; break; default: qWarning("UrlFactory::getImageFormat() Unknown map id %d", type); @@ -563,7 +560,7 @@ UrlFactory::_tryCorrectGoogleVersions(QNetworkAccessManager* networkManager) #define AVERAGE_MAPBOX_SAT_MAP 15739 #define AVERAGE_MAPBOX_STREET_MAP 5648 #define AVERAGE_TILE_SIZE 13652 -#define AVERAGE_AIRMAP_ELEV_SIZE 5360 +#define AVERAGE_AIRMAP_ELEV_SIZE 2786 //----------------------------------------------------------------------------- quint32 diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 80a16e7..1940798 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -398,6 +398,8 @@ void TerrainTileManager::_fetchedTile() // parse received data and insert into hash table QByteArray responseBytes = reply->mapImageData(); + qWarning() << "Received some bytes of terrain data: " << responseBytes.size(); + TerrainTile* terrainTile = new TerrainTile(responseBytes); if (terrainTile->isValid()) { _tilesMutex.lock(); diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h index 96d98e2..bc5ee91 100644 --- a/src/Terrain/TerrainQuery.h +++ b/src/Terrain/TerrainQuery.h @@ -87,7 +87,7 @@ private: bool _carpetStatsOnly; }; -/// AirMap online implementation of terrain queries +/// AirMap offline cachable implementation of terrain queries class TerrainOfflineAirMapQuery : public TerrainQueryInterface { Q_OBJECT @@ -99,12 +99,10 @@ public: void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) final; void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) final; - // Internal method + // Internal methods void _signalCoordinateHeights(bool success, QList heights); void _signalPathHeights(bool success, double latStep, double lonStep, const QList& heights); void _signalCarpetHeights(bool success, double minHeight, double maxHeight, const QList>& carpet); - - bool _carpetStatsOnly; }; /// Used internally by TerrainOfflineAirMapQuery to manage terrain tiles diff --git a/src/TerrainTile.cc b/src/TerrainTile.cc index 8e6a6d5..e95bf89 100644 --- a/src/TerrainTile.cc +++ b/src/TerrainTile.cc @@ -55,31 +55,38 @@ TerrainTile::TerrainTile(QByteArray byteArray) { QDataStream stream(byteArray); - double lat,lon; + float lat,lon; stream >> lat - >> lon; + >> lon; _southWest.setLatitude(lat); _southWest.setLongitude(lon); stream >> lat - >> lon; + >> lon; _northEast.setLatitude(lat); _northEast.setLongitude(lon); stream >> _minElevation - >> _maxElevation - >> _avgElevation - >> _gridSizeLat - >> _gridSizeLon; + >> _maxElevation + >> _avgElevation + >> _gridSizeLat + >> _gridSizeLon; + + qCDebug(TerrainTileLog) << "Loading terrain tile: " << _southWest << " - " << _northEast; + qCDebug(TerrainTileLog) << "min:max:avg:sizeLat:sizeLon" << _minElevation << _maxElevation << _avgElevation << _gridSizeLat << _gridSizeLon; for (int i = 0; i < _gridSizeLat; i++) { if (i == 0) { - _data = new double*[_gridSizeLat]; + _data = new int16_t*[_gridSizeLat]; for (int k = 0; k < _gridSizeLat; k++) { - _data[k] = new double[_gridSizeLon]; + _data[k] = new int16_t[_gridSizeLon]; } } for (int j = 0; j < _gridSizeLon; j++) { + if (stream.atEnd()) { + qWarning() << "Terrain tile binary data does not contain all data"; + return; + } stream >> _data[i][j]; } } @@ -108,7 +115,7 @@ double TerrainTile::elevation(const QGeoCoordinate& coordinate) const int indexLat = _latToDataIndex(coordinate.latitude()); int indexLon = _lonToDataIndex(coordinate.longitude()); qCDebug(TerrainTileLog) << "indexLat:indexLon" << indexLat << indexLon << "elevation" << _data[indexLat][indexLon]; - return _data[indexLat][indexLon]; + return static_cast(_data[indexLat][indexLon]); } else { qCDebug(TerrainTileLog) << "Asking for elevation, but no valid data."; return -1.0; @@ -141,7 +148,7 @@ QByteArray TerrainTile::serialize(QByteArray input) QString errorString; QList rootVersionKeyInfoList = { { _jsonStatusKey, QJsonValue::String, true }, - { _jsonDataKey, QJsonValue::Object, true }, + { _jsonDataKey, QJsonValue::Object, true }, }; if (!JsonHelper::validateKeys(rootObject, rootVersionKeyInfoList, errorString)) { qCDebug(TerrainTileLog) << "Error in reading json: " << errorString; @@ -157,7 +164,7 @@ QByteArray TerrainTile::serialize(QByteArray input) const QJsonObject& dataObject = rootObject[_jsonDataKey].toObject(); QList dataVersionKeyInfoList = { { _jsonBoundsKey, QJsonValue::Object, true }, - { _jsonStatsKey, QJsonValue::Object, true }, + { _jsonStatsKey, QJsonValue::Object, true }, { _jsonCarpetKey, QJsonValue::Array, true }, }; if (!JsonHelper::validateKeys(dataObject, dataVersionKeyInfoList, errorString)) { @@ -184,16 +191,16 @@ QByteArray TerrainTile::serialize(QByteArray input) QByteArray emptyArray; return emptyArray; } - stream << swArray[0].toDouble(); - stream << swArray[1].toDouble(); - stream << neArray[0].toDouble(); - stream << neArray[1].toDouble(); + stream << static_cast(swArray[0].toDouble()); + stream << static_cast(swArray[1].toDouble()); + stream << static_cast(neArray[0].toDouble()); + stream << static_cast(neArray[1].toDouble()); // Stats const QJsonObject& statsObject = dataObject[_jsonStatsKey].toObject(); QList statsVersionKeyInfoList = { - { _jsonMaxElevationKey, QJsonValue::Double, true }, { _jsonMinElevationKey, QJsonValue::Double, true }, + { _jsonMaxElevationKey, QJsonValue::Double, true }, { _jsonAvgElevationKey, QJsonValue::Double, true }, }; if (!JsonHelper::validateKeys(statsObject, statsVersionKeyInfoList, errorString)) { @@ -201,21 +208,21 @@ QByteArray TerrainTile::serialize(QByteArray input) QByteArray emptyArray; return emptyArray; } - stream << statsObject[_jsonMaxElevationKey].toInt(); - stream << statsObject[_jsonMinElevationKey].toInt(); - stream << statsObject[_jsonAvgElevationKey].toDouble(); + stream << static_cast(statsObject[_jsonMinElevationKey].toInt()); + stream << static_cast(statsObject[_jsonMaxElevationKey].toInt()); + stream << static_cast(statsObject[_jsonAvgElevationKey].toDouble()); // Carpet const QJsonArray& carpetArray = dataObject[_jsonCarpetKey].toArray(); int gridSizeLat = carpetArray.count(); - stream << gridSizeLat; + stream << static_cast(gridSizeLat); int gridSizeLon = 0; qCDebug(TerrainTileLog) << "Received tile has size in latitude direction: " << carpetArray.count(); for (int i = 0; i < gridSizeLat; i++) { const QJsonArray& row = carpetArray[i].toArray(); if (i == 0) { gridSizeLon = row.count(); - stream << gridSizeLon; + stream << static_cast(gridSizeLon); qCDebug(TerrainTileLog) << "Received tile has size in longitued direction: " << row.count(); } if (row.count() < gridSizeLon) { @@ -224,7 +231,7 @@ QByteArray TerrainTile::serialize(QByteArray input) return emptyArray; } for (int j = 0; j < gridSizeLon; j++) { - stream << row[j].toDouble(); + stream << static_cast(row[j].toDouble()); } } diff --git a/src/TerrainTile.h b/src/TerrainTile.h index 7db6679..adf217d 100644 --- a/src/TerrainTile.h +++ b/src/TerrainTile.h @@ -101,13 +101,13 @@ private: QGeoCoordinate _southWest; /// South west corner of the tile QGeoCoordinate _northEast; /// North east corner of the tile - int _minElevation; /// Minimum elevation in tile - int _maxElevation; /// Maximum elevation in tile - double _avgElevation; /// Average elevation of the tile + int16_t _minElevation; /// Minimum elevation in tile + int16_t _maxElevation; /// Maximum elevation in tile + float _avgElevation; /// Average elevation of the tile - double** _data; /// 2D elevation data array - int _gridSizeLat; /// data grid size in latitude direction - int _gridSizeLon; /// data grid size in longitude direction + int16_t** _data; /// 2D elevation data array + int16_t _gridSizeLat; /// data grid size in latitude direction + int16_t _gridSizeLon; /// data grid size in longitude direction bool _isValid; /// data loaded is valid // Json keys From 6c96642c9e10dc5fca951e379eb1a4606bb27120 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Wed, 21 Mar 2018 14:14:14 +0100 Subject: [PATCH 28/29] make mac CI happy --- src/Terrain/TerrainQuery.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Terrain/TerrainQuery.cc b/src/Terrain/TerrainQuery.cc index 1940798..f5cbee3 100644 --- a/src/Terrain/TerrainQuery.cc +++ b/src/Terrain/TerrainQuery.cc @@ -21,6 +21,7 @@ #include #include #include +#include QGC_LOGGING_CATEGORY(TerrainQueryLog, "TerrainQueryLog") From 9b71c8fea344748db0e3550ef0089bb0702d7212 Mon Sep 17 00:00:00 2001 From: Andreas Bircher Date: Thu, 22 Mar 2018 07:53:00 +0100 Subject: [PATCH 29/29] Switch over to offline cachable queries for coordinate and path --- src/Terrain/TerrainQuery.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Terrain/TerrainQuery.h b/src/Terrain/TerrainQuery.h index bc5ee91..ee07763 100644 --- a/src/Terrain/TerrainQuery.h +++ b/src/Terrain/TerrainQuery.h @@ -188,7 +188,7 @@ private: State _state = State::Idle; const int _batchTimeout = 500; QTimer _batchTimer; - TerrainAirMapQuery _terrainQuery; + TerrainOfflineAirMapQuery _terrainQuery; }; /// NOTE: TerrainAtCoordinateQuery is not thread safe. All instances/calls to ElevationProvider must be on main thread. @@ -236,7 +236,7 @@ private slots: void _pathHeights(bool success, double latStep, double lonStep, const QList& heights); private: - TerrainAirMapQuery _terrainQuery; + TerrainOfflineAirMapQuery _terrainQuery; }; Q_DECLARE_METATYPE(TerrainPathQuery::PathHeightInfo_t)