Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 37 additions & 3 deletions src/Terrain/TerrainTileManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include "QGCLoggingCategory.h"
#include "QGCGeo.h"

#include <QtCore/QDateTime>
#include <QtLocation/private/qgeotilespec_p.h>
#include <QtNetwork/QNetworkAccessManager>
#include <QtNetwork/QNetworkRequest>
Expand Down Expand Up @@ -72,6 +73,11 @@ bool TerrainTileManager::getAltitudesForCoordinates(const QList<QGeoCoordinate>
qCDebug(TerrainTileManagerLog) << "returning elevation from tile cache" << elevation;
}
altitudes.push_back(elevation);
} else if (_isFailedTile(tileHash)) {
// Tile fetch failed recently; short-circuit to avoid hammering the server with repeated requests
// (e.g. uninitialized 0,0 coordinates from MAVLink TERRAIN_REQUEST returning HTTP 500).
error = true;
altitudes.push_back(qQNaN());
} else if (_state != TerrainQuery::State::Downloading) {
QGeoTileSpec spec;
spec.setX(provider->long2tileX(coordinate.longitude(), 1));
Expand Down Expand Up @@ -295,21 +301,36 @@ void TerrainTileManager::_terrainDone()
const QByteArray responseBytes = reply->mapImageData();
const QGeoTileSpec spec = reply->tileSpec();

const QString hash = UrlFactory::getTileHash(UrlFactory::getProviderTypeFromQtMapId(spec.mapId()), spec.x(), spec.y(), spec.zoom());

if (reply->error() != QGeoTiledMapReplyQGC::NoError) {
qCWarning(TerrainTileManagerLog) << "Elevation tile fetching returned error:" << reply->errorString();
const bool firstFailure = !_failedTiles.contains(hash);
_failedTiles.insert(hash, QDateTime::currentMSecsSinceEpoch());
if (firstFailure) {
qCWarning(TerrainTileManagerLog) << "Elevation tile fetching returned error:" << reply->errorString();
} else {
qCDebug(TerrainTileManagerLog) << "Elevation tile fetching returned error (suppressed):" << reply->errorString();
}
_tileFailed();
return;
}

if (responseBytes.isEmpty()) {
qCWarning(TerrainTileManagerLog) << "Error in fetching elevation tile. Empty response.";
const bool firstFailure = !_failedTiles.contains(hash);
_failedTiles.insert(hash, QDateTime::currentMSecsSinceEpoch());
if (firstFailure) {
qCWarning(TerrainTileManagerLog) << "Error in fetching elevation tile. Empty response.";
} else {
qCDebug(TerrainTileManagerLog) << "Error in fetching elevation tile. Empty response (suppressed).";
}
_tileFailed();
return;
}

_failedTiles.remove(hash);

qCDebug(TerrainTileManagerLog) << "Received some bytes of terrain data:" << responseBytes.size();

const QString hash = UrlFactory::getTileHash(UrlFactory::getProviderTypeFromQtMapId(spec.mapId()), spec.x(), spec.y(), spec.zoom());
_cacheTile(responseBytes, hash);

for (qsizetype i = _requestQueue.count() - 1; i >= 0; i--) {
Expand Down Expand Up @@ -402,6 +423,19 @@ TerrainTile *TerrainTileManager::_getCachedTile(const QString &hash)
return tile;
}

bool TerrainTileManager::_isFailedTile(const QString &hash)
{
const auto it = _failedTiles.constFind(hash);
if (it == _failedTiles.constEnd()) {
return false;
}
if (QDateTime::currentMSecsSinceEpoch() - it.value() < kFailedTileBackoffMs) {
return true;
}
_failedTiles.erase(it);
return false;
}

void TerrainTileManager::_processCarpetResults(const QList<double> &altitudes, int gridSizeLat, int gridSizeLon,
bool statsOnly, double &minHeight, double &maxHeight, QList<QList<double>> &carpet)
{
Expand Down
4 changes: 4 additions & 0 deletions src/Terrain/TerrainTileManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ private slots:
void _tileFailed();
void _cacheTile(const QByteArray &data, const QString &hash);
TerrainTile *_getCachedTile(const QString &hash);
bool _isFailedTile(const QString &hash);
static void _processCarpetResults(const QList<double> &altitudes, int gridSizeLat, int gridSizeLon,
bool statsOnly, double &minHeight, double &maxHeight, QList<QList<double>> &carpet);

Expand All @@ -59,6 +60,9 @@ private slots:

QMutex _tilesMutex;
QHash<QString, TerrainTile*> _tiles;
QHash<QString, qint64> _failedTiles; ///< Tile hash -> ms since epoch of last failed fetch; suppresses immediate retries

QNetworkAccessManager *_networkManager = nullptr;

static constexpr qint64 kFailedTileBackoffMs = 5000;
};
25 changes: 19 additions & 6 deletions src/Vehicle/TerrainProtocolHandler.cc
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,18 @@ bool TerrainProtocolHandler::mavlinkMessageReceived(const mavlink_message_t &mes

void TerrainProtocolHandler::_handleTerrainRequest(const mavlink_message_t &message)
{
mavlink_terrain_request_t terrainRequest;
mavlink_msg_terrain_request_decode(&message, &terrainRequest);

// Defensive drop: autopilots can emit TERRAIN_REQUEST with uninitialized (0, 0)
// before GPS lock; honoring it just spams the elevation server with tile-(0,0) fetches.
if (terrainRequest.lat == 0 && terrainRequest.lon == 0) {
qCDebug(TerrainProtocolHandlerLog) << "Ignoring TERRAIN_REQUEST with lat=0, lon=0";
return;
}

_currentTerrainRequest = terrainRequest;
_terrainRequestActive = true;
mavlink_msg_terrain_request_decode(&message, &_currentTerrainRequest);
_sendNextTerrainData();
}

Expand Down Expand Up @@ -137,14 +147,17 @@ void TerrainProtocolHandler::_sendTerrainData(const QGeoCoordinate &swCorner, ui
return;
}

// Clear the bit on error or success. On error (e.g. tile fetch failed at the server),
// looping on the bit would just hammer the same failed tile every timer tick. The vehicle
// will re-issue TERRAIN_REQUEST if it still needs the data, allowing a fresh attempt after
// TerrainTileManager's failed-tile backoff expires.
const uint64_t removeBit = ~(1ull << gridBit);
_currentTerrainRequest.mask &= removeBit;

if (error) {
qCWarning(TerrainProtocolHandlerLog) << Q_FUNC_INFO << "TerrainAtCoordinateQuery::getAltitudesForCoordinates failed";
qCDebug(TerrainProtocolHandlerLog) << Q_FUNC_INFO << "terrain data unavailable for gridBit" << gridBit;
return;
}

// Only clear the bit if the query succeeds. Otherwise just let it try again on the next timer tick
const uint64_t removeBit = ~(1ull << gridBit);
_currentTerrainRequest.mask &= removeBit;
int altIndex = 0;
int16_t terrainData[16];
for (const double& altitude : altitudes) {
Expand Down
Loading