RemoteID/PositionManager: fix GCS Live GNSS position

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());
 

src/PositionManager/PositionManager.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <QtCore/QDateTime>
 #include <QtCore/QObject>
 #include <QtPositioning/QGeoCoordinate>
 #include <QtPositioning/QGeoPositionInfo>
@@ -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<qreal>::infinity();
     qreal _gcsPositionVerticalAccuracy = std::numeric_limits<qreal>::infinity();

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<int>(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();
-    }
-}

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;