diff --git a/src/PositionManager/PositionManager.cpp b/src/PositionManager/PositionManager.cpp index 96a07de0f2d..e69099b4066 100644 --- a/src/PositionManager/PositionManager.cpp +++ b/src/PositionManager/PositionManager.cpp @@ -134,6 +134,10 @@ void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update) if (_gcsPositionHorizontalAccuracy <= kMinHorizonalAccuracyMeters) { newGCSPosition.setLatitude(update.coordinate().latitude()); newGCSPosition.setLongitude(update.coordinate().longitude()); + // Stamp the local arrival time so consumers can tell how fresh gcsPosition is. + // Updates rejected by the accuracy gate leave the stamp alone, since they leave + // the previous coordinate in place as well. + _gcsPositionTimestamp = QDateTime::currentDateTimeUtc(); } emit gcsPositionHorizontalAccuracyChanged(_gcsPositionHorizontalAccuracy); } @@ -197,6 +201,7 @@ void QGCPositionManager::_setPositionSource(QGCPositionSource source) emit positionInfoUpdated(_geoPositionInfo); _setGCSPosition(QGeoCoordinate()); + _gcsPositionTimestamp = QDateTime(); _setGCSHeading(qQNaN()); diff --git a/src/PositionManager/PositionManager.h b/src/PositionManager/PositionManager.h index 7bc4c749a9d..a45aa626dae 100644 --- a/src/PositionManager/PositionManager.h +++ b/src/PositionManager/PositionManager.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -34,6 +35,13 @@ class QGCPositionManager : public QObject QGeoPositionInfo geoPositionInfo() const { return _geoPositionInfo; } QGeoPositionInfoSource::Error gcsPositioningError() const { return _gcsPositioningError; } + /// Local arrival time of the last position update which passed the accuracy gates and was + /// copied into gcsPosition. Invalid until the first such update arrives. This is the local + /// clock rather than the position source's own timestamp, which on some platforms (e.g. + /// Android) is offset from the system clock. + /// @return Arrival time, in UTC, of the last position update applied to gcsPosition. + QDateTime gcsPositionTimestamp() const { return _gcsPositionTimestamp; } + int updateInterval() const { return _updateInterval; } void setNmeaSourceDevice(QIODevice *device); @@ -74,6 +82,7 @@ private slots: QGeoPositionInfoSource::Error _gcsPositioningError = QGeoPositionInfoSource::NoError; QGeoCoordinate _gcsPosition; + QDateTime _gcsPositionTimestamp; qreal _gcsHeading = qQNaN(); qreal _gcsPositionHorizontalAccuracy = std::numeric_limits::infinity(); qreal _gcsPositionVerticalAccuracy = std::numeric_limits::infinity(); diff --git a/src/Vehicle/RemoteIDManager.cc b/src/Vehicle/RemoteIDManager.cc index c5c7598e0d8..d367f42f2ec 100644 --- a/src/Vehicle/RemoteIDManager.cc +++ b/src/Vehicle/RemoteIDManager.cc @@ -45,9 +45,6 @@ RemoteIDManager::RemoteIDManager(Vehicle* vehicle) _sendMessagesTimer.setInterval(SENDING_RATE_MSEC); connect(&_sendMessagesTimer, &QTimer::timeout, this, &RemoteIDManager::_sendMessages); - // GCS GPS position updates to track the health of the GPS data - connect(QGCPositionManager::instance(), &QGCPositionManager::positionInfoUpdated, this, &RemoteIDManager::_updateLastGCSPositionInfo); - // Assign vehicle sysid and compid. GCS must target these messages to autopilot, and autopilot will redirect them to RID device _targetSystem = _vehicle->id(); _targetComponent = _vehicle->compId(); @@ -302,11 +299,22 @@ void RemoteIDManager::_sendSystem() QGCPositionManager* positionManager = QGCPositionManager::instance(); QGeoPositionInfo geoPositionInfo = positionManager->geoPositionInfo(); gcsPosition = positionManager->gcsPosition(); + const QDateTime gcsPositionTimestamp = positionManager->gcsPositionTimestamp(); + + // gcsPosition only carries an altitude when the fix's vertical accuracy is within the + // strict gate QGCPositionManager applies for consumers which act on it, such as Follow Me + // and update-home-position. Remote ID mandates an operator altitude in FAA regions and + // OPEN_DRONE_ID_SYSTEM has no accuracy field for it, so a loosely known altitude is better + // than none here: take it straight from the fix whenever the fix reports one. + const QGeoCoordinate fixCoordinate = geoPositionInfo.coordinate(); + if (fixCoordinate.type() == QGeoCoordinate::Coordinate3D) { + gcsPosition.setAltitude(fixCoordinate.altitude()); + } if (!geoPositionInfo.isValid()) { // Only warn if we've previously received a valid fix; otherwise the source is // still initializing and the absence of data is expected, not an error. - _updateGcsPositionStatus(false, _lastGeoPositionTimeStamp.isValid() + _updateGcsPositionStatus(false, gcsPositionTimestamp.isValid() ? QStringLiteral("GCS GPS data is not valid.") : QString()); } else if (positionManager->gcsPositioningError() != QGeoPositionInfoSource::NoError && positionManager->gcsPositioningError() != QGeoPositionInfoSource::UpdateTimeoutError) { @@ -316,7 +324,7 @@ void RemoteIDManager::_sendSystem() } else if (_settings->region()->rawValue().toInt() == static_cast(RemoteIDSettings::RegionOperation::FAA) && gcsPosition.type() != QGeoCoordinate::Coordinate3D) { // FAA requires altitude data, or else the GPS data is not good _updateGcsPositionStatus(false, "GCS GPS data error: Altitude data is mandatory for FAA regions."); - } else if (_lastGeoPositionTimeStamp.msecsTo(QDateTime::currentDateTime().currentDateTimeUtc()) > ALLOWED_GPS_DELAY) { + } else if (!gcsPositionTimestamp.isValid() || (gcsPositionTimestamp.msecsTo(QDateTime::currentDateTimeUtc()) > ALLOWED_GPS_DELAY)) { _updateGcsPositionStatus(false, "GCS GPS data is older than 5 seconds"); } else { _updateGcsPositionStatus(true); @@ -399,10 +407,3 @@ void RemoteIDManager::setEmergency(bool declare) qCDebug(RemoteIDManagerLog) << ( declare ? "Emergency declared." : "Emergency cleared."); } - -void RemoteIDManager::_updateLastGCSPositionInfo(QGeoPositionInfo update) -{ - if (update.isValid()) { - _lastGeoPositionTimeStamp = update.timestamp().toUTC(); - } -} diff --git a/src/Vehicle/RemoteIDManager.h b/src/Vehicle/RemoteIDManager.h index db25bdf2e76..5c8d8b22f1f 100644 --- a/src/Vehicle/RemoteIDManager.h +++ b/src/Vehicle/RemoteIDManager.h @@ -59,7 +59,6 @@ class RemoteIDManager : public QObject private slots: void _odidTimeout(); void _sendMessages(); - void _updateLastGCSPositionInfo(QGeoPositionInfo update); private: void _handleArmStatus(mavlink_message_t& message); @@ -94,7 +93,6 @@ private slots: bool _vehicleReportsBasicIDMissing; bool _emergencyDeclared; - QDateTime _lastGeoPositionTimeStamp; int _targetSystem; int _targetComponent;