diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index ddad562..77c99a7 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)
 {
@@ -783,13 +784,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 3ec98e4..c5506db 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;
 
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 018611c..fd6cfb7 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1425,6 +1425,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_NAMED_VALUE_INT:
         case MAVLINK_MSG_ID_MANUAL_CONTROL:
         case MAVLINK_MSG_ID_HIGHRES_IMU:
+        case MAVLINK_MSG_ID_DISTANCE_SENSOR:
             break;
         default:
         {