diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 10741526c804ab306a1c64b711d38f7b319fc015..fc5ad717715aa9e98bb86f75bc65bcef13d67b0b 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1031,25 +1031,33 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
     if (message->GetTypeName() == pointCloud.GetTypeName())
     {
         receivedPointCloudTimestamp = QGC::groundTimeSeconds();
+        pointCloudMutex.lock();
         pointCloud.CopyFrom(*message);
+        pointCloudMutex.unlock();
         emit pointCloudChanged(this);
     }
     else if (message->GetTypeName() == rgbdImage.GetTypeName())
     {
         receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
+        rgbdImageMutex.lock();
         rgbdImage.CopyFrom(*message);
+        rgbdImageMutex.unlock();
         emit rgbdImageChanged(this);
     }
     else if (message->GetTypeName() == obstacleList.GetTypeName())
     {
         receivedObstacleListTimestamp = QGC::groundTimeSeconds();
+        obstacleListMutex.lock();
         obstacleList.CopyFrom(*message);
+        obstacleListMutex.unlock();
         emit obstacleListChanged(this);
     }
     else if (message->GetTypeName() == path.GetTypeName())
     {
         receivedPathTimestamp = QGC::groundTimeSeconds();
+        pathMutex.lock();
         path.CopyFrom(*message);
+        pathMutex.unlock();
         emit pathChanged(this);
     }
 }
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index cd98357dfc368f96bf29ed169a2d784d4a8ffc9b..e74af17f7180131df2e445229b64710faf28696d 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -135,39 +135,47 @@ public:
     bool getSelected() const;
 
 #ifdef QGC_PROTOBUF_ENABLED
-    px::PointCloudXYZRGB getPointCloud() const {
+    px::PointCloudXYZRGB getPointCloud() {
+        QMutexLocker locker(&pointCloudMutex);
         return pointCloud;
     }
 
-    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) const {
+    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) {
         receivedTimestamp = receivedPointCloudTimestamp;
+        QMutexLocker locker(&pointCloudMutex);
         return pointCloud;
     }
 
-    px::RGBDImage getRGBDImage() const {
+    px::RGBDImage getRGBDImage() {
+        QMutexLocker locker(&rgbdImageMutex);
         return rgbdImage;
     }
 
-    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) const {
+    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) {
         receivedTimestamp = receivedRGBDImageTimestamp;
+        QMutexLocker locker(&rgbdImageMutex);
         return rgbdImage;
     }
 
-    px::ObstacleList getObstacleList() const {
+    px::ObstacleList getObstacleList() {
+        QMutexLocker locker(&obstacleListMutex);
         return obstacleList;
     }
 
-    px::ObstacleList getObstacleList(qreal& receivedTimestamp) const {
+    px::ObstacleList getObstacleList(qreal& receivedTimestamp) {
         receivedTimestamp = receivedObstacleListTimestamp;
+        QMutexLocker locker(&obstacleListMutex);
         return obstacleList;
     }
 
-    px::Path getPath() const {
+    px::Path getPath() {
+        QMutexLocker locker(&pathMutex);
         return path;
     }
 
-    px::Path getPath(qreal& receivedTimestamp) const {
+    px::Path getPath(qreal& receivedTimestamp) {
         receivedTimestamp = receivedPathTimestamp;
+        QMutexLocker locker(&pathMutex);
         return path;
     }
 #endif
@@ -257,15 +265,19 @@ protected: //COMMENTS FOR TEST UNIT
 
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
+    QMutex pointCloudMutex;
     qreal receivedPointCloudTimestamp;
 
     px::RGBDImage rgbdImage;
+    QMutex rgbdImageMutex;
     qreal receivedRGBDImageTimestamp;
 
     px::ObstacleList obstacleList;
+    QMutex obstacleListMutex;
     qreal receivedObstacleListTimestamp;
 
     px::Path path;
+    QMutex pathMutex;
     qreal receivedPathTimestamp;
 #endif
 
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 4721001e2a92f49f11b6fa208e65263ad9777e19..3c6ba920b2713ecedc13c8ae30217e6150e6b04e 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -95,14 +95,14 @@ public:
     virtual bool getSelected() const = 0;
 
 #ifdef QGC_PROTOBUF_ENABLED
-    virtual px::PointCloudXYZRGB getPointCloud() const = 0;
-    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) const = 0;
-    virtual px::RGBDImage getRGBDImage() const = 0;
-    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) const = 0;
-    virtual px::ObstacleList getObstacleList() const = 0;
-    virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) const = 0;
-    virtual px::Path getPath() const = 0;
-    virtual px::Path getPath(qreal& receivedTimestamp) const = 0;
+    virtual px::PointCloudXYZRGB getPointCloud() = 0;
+    virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) = 0;
+    virtual px::RGBDImage getRGBDImage() = 0;
+    virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) = 0;
+    virtual px::ObstacleList getObstacleList() = 0;
+    virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) = 0;
+    virtual px::Path getPath() = 0;
+    virtual px::Path getPath(qreal& receivedTimestamp) = 0;
 #endif
 
     virtual bool isArmed() const = 0;
diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc
index f29ab5e8b50bb62fe66cce6aced4af0126b139a5..cfa24beac5d0e7d5cd1cf859771b8cf8db5487c0 100644
--- a/src/ui/QGCRGBDView.cc
+++ b/src/ui/QGCRGBDView.cc
@@ -52,7 +52,6 @@ void QGCRGBDView::clearData(void)
     qDebug() << offlineImg.load(":/images/status/colorbars.png");
 
     glImage = QGLWidget::convertToGLFormat(offlineImg);
-    qDebug() << "cleardata" << offlineImg.isNull() << offlineImg.width() << offlineImg.height();
 }
 
 void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event)