diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index ef9dfd0c3f9c914e04daa163d09b1a5d080269f4..c170d2bf64d9c8de6e577ff1a4a369b7c8e724e4 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -55,6 +55,7 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     simUpdateLast(QGC::groundTimeMilliseconds()),
     simUpdateFirst(0),
     simUpdateLastText(QGC::groundTimeMilliseconds()),
+    simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
     simUpdateHz(0),
     _sensorHilEnabled(true)
 {
@@ -714,13 +715,13 @@ void QGCXPlaneLink::readBytes()
         }
 
         // Limit ground truth to 25 Hz
-        if (QGC::groundTimeMilliseconds() - simUpdateLast > 40) {
+        if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
             emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                                        pitchspeed, yawspeed, lat, lon, alt,
                                        vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
-        }
 
-        simUpdateLast = QGC::groundTimeMilliseconds();
+            simUpdateLastGroundTruth = QGC::groundTimeMilliseconds();
+        }
     }
 
     if (!oldConnectionState && xPlaneConnected)
diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h
index f80110f968802d621d74d88b47fbf894ee60a4e8..05c480e7c2e1f578697aa22521508e19732f03c2 100644
--- a/src/comm/QGCXPlaneLink.h
+++ b/src/comm/QGCXPlaneLink.h
@@ -207,6 +207,7 @@ protected:
     quint64 simUpdateLast;
     quint64 simUpdateFirst;
     quint64 simUpdateLastText;
+    quint64 simUpdateLastGroundTruth;
     float simUpdateHz;
     bool _sensorHilEnabled;