diff --git a/images/status/colorbars.png b/images/status/colorbars.png
new file mode 100644
index 0000000000000000000000000000000000000000..29320d193256ade753ccf033b080928161140a2a
Binary files /dev/null and b/images/status/colorbars.png differ
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 524119c4dbd7e53d721f4fc758ef01d54c966738..1c7619edab995e054b62a874c3ee88ebf66071d9 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -75,6 +75,7 @@
         <file>images/status/audio-volume-medium.svg</file>
         <file>images/status/audio-volume-low.svg</file>
         <file>images/status/audio-volume-high.svg</file>
+	<file>images/status/colorbars.png</file>
         <file>images/style-mission.css</file>
         <file>images/splash.png</file>
         <file>audio/alert.wav</file>
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 68e8fc6441a96cbdb01e21aac7e52dc773370435..2dbac508f81c840d07b9a70b9bc668c14826fc6f 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -75,6 +75,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     pitch(0.0),
     yaw(0.0),
     statusTimeout(new QTimer(this)),
+#ifdef QGC_PROTOBUF_ENABLED
+    receivedPointCloudTimestamp(0.0),
+    receivedRGBDImageTimestamp(0.0),
+    receivedObstacleListTimestamp(0.0),
+    receivedPathTimestamp(0.0),
+#endif
     paramsOnceRequested(false),
     airframe(QGC_AIRFRAME_EASYSTAR),
     attitudeKnown(false),
@@ -1027,21 +1033,25 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
 
     if (message->GetTypeName() == pointCloud.GetTypeName())
     {
+        receivedPointCloudTimestamp = QGC::groundTimeSeconds();
         pointCloud.CopyFrom(*message);
         emit pointCloudChanged(this);
     }
     else if (message->GetTypeName() == rgbdImage.GetTypeName())
     {
+        receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
         rgbdImage.CopyFrom(*message);
         emit rgbdImageChanged(this);
     }
     else if (message->GetTypeName() == obstacleList.GetTypeName())
     {
+        receivedObstacleListTimestamp = QGC::groundTimeSeconds();
         obstacleList.CopyFrom(*message);
         emit obstacleListChanged(this);
     }
     else if (message->GetTypeName() == path.GetTypeName())
     {
+        receivedPathTimestamp = QGC::groundTimeSeconds();
         path.CopyFrom(*message);
         emit pathChanged(this);
     }
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index 290025bb1c8caac436b7494db67556f72dc61def..cd98357dfc368f96bf29ed169a2d784d4a8ffc9b 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -139,17 +139,37 @@ public:
         return pointCloud;
     }
 
+    px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedPointCloudTimestamp;
+        return pointCloud;
+    }
+
     px::RGBDImage getRGBDImage() const {
         return rgbdImage;
     }
 
+    px::RGBDImage getRGBDImage(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedRGBDImageTimestamp;
+        return rgbdImage;
+    }
+
     px::ObstacleList getObstacleList() const {
         return obstacleList;
     }
 
+    px::ObstacleList getObstacleList(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedObstacleListTimestamp;
+        return obstacleList;
+    }
+
     px::Path getPath() const {
         return path;
     }
+
+    px::Path getPath(qreal& receivedTimestamp) const {
+        receivedTimestamp = receivedPathTimestamp;
+        return path;
+    }
 #endif
 
     friend class UASWaypointManager;
@@ -237,9 +257,16 @@ protected: //COMMENTS FOR TEST UNIT
 
 #ifdef QGC_PROTOBUF_ENABLED
     px::PointCloudXYZRGB pointCloud;
+    qreal receivedPointCloudTimestamp;
+
     px::RGBDImage rgbdImage;
+    qreal receivedRGBDImageTimestamp;
+
     px::ObstacleList obstacleList;
+    qreal receivedObstacleListTimestamp;
+
     px::Path path;
+    qreal receivedPathTimestamp;
 #endif
 
     QMap<int, QMap<QString, QVariant>* > parameters; ///< All parameters
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 37df27f043e58593ccfd1ebee88f8e2e7cb7ea29..4721001e2a92f49f11b6fa208e65263ad9777e19 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -96,9 +96,13 @@ public:
 
 #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;
 #endif
 
     virtual bool isArmed() const = 0;
diff --git a/src/ui/HUD.h b/src/ui/HUD.h
index 1f5eebdbdacc5f74b92a0fc9f04cc6ebef81feb1..5239808d1265615f77d08fe3a8acbdea05d2894a 100644
--- a/src/ui/HUD.h
+++ b/src/ui/HUD.h
@@ -61,7 +61,7 @@ public slots:
     //void paintGL();
 
     /** @brief Set the currently monitored UAS */
-    void setActiveUAS(UASInterface* uas);
+    virtual void setActiveUAS(UASInterface* uas);
 
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
 //    void updateAttitudeThrustSetPoint(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc
index ded4a4edac8b2a3808e8d99f0d8e364d36232f8b..f29ab5e8b50bb62fe66cce6aced4af0126b139a5 100644
--- a/src/ui/QGCRGBDView.cc
+++ b/src/ui/QGCRGBDView.cc
@@ -21,16 +21,41 @@ QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) :
     enableDepthAction->setChecked(depthEnabled);
     connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool)));
 
-    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+
+    clearData();
+}
+
+void QGCRGBDView::setActiveUAS(UASInterface* uas)
+{
+    if (this->uas != NULL)
+    {
+        // Disconnect any previously connected active MAV
+        disconnect(this->uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+
+        clearData();
+    }
+
+    if (uas)
+    {
+        // Now connect the new UAS
+        // Setup communication
+        connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    }
+
+    HUD::setActiveUAS(uas);
 }
 
-void QGCRGBDView::addUAS(UASInterface *uas)
+void QGCRGBDView::clearData(void)
 {
-    // TODO Enable multi-uas support
-    connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*)));
+    QImage offlineImg;
+    qDebug() << offlineImg.load(":/images/status/colorbars.png");
+
+    glImage = QGLWidget::convertToGLFormat(offlineImg);
+    qDebug() << "cleardata" << offlineImg.isNull() << offlineImg.width() << offlineImg.height();
 }
 
-void QGCRGBDView::contextMenuEvent (QContextMenuEvent* event)
+void QGCRGBDView::contextMenuEvent(QContextMenuEvent* event)
 {
     QMenu menu(this);
     // Update actions
@@ -248,7 +273,7 @@ void QGCRGBDView::updateData(UASInterface *uas)
             {
                 if (depth[c] != 0)
                 {
-                    int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f;
+                    int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f;
                     idx = 127 - idx;
 
                     pixel[0] = colormapJet[idx][2] * 255.0f;
diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h
index 586bba7623d5670846d5d397c2b2709119e5b7a2..2ca599feb77e7a4307138c2408d2e02db28df0df 100644
--- a/src/ui/QGCRGBDView.h
+++ b/src/ui/QGCRGBDView.h
@@ -12,7 +12,9 @@ public:
 signals:
 
 public slots:
-    void addUAS(UASInterface* uas);
+    void setActiveUAS(UASInterface* uas);
+
+    void clearData(void);
     void enableRGB(bool enabled);
     void enableDepth(bool enabled);
     void updateData(UASInterface *uas);
diff --git a/src/ui/map3D/ObstacleGroupNode.cc b/src/ui/map3D/ObstacleGroupNode.cc
index f0942a97d46e1dde61f4fef5e8cf78175c007e89..94c208f7a8a83204ff6f3ca862e0165fd101bfaa 100644
--- a/src/ui/map3D/ObstacleGroupNode.cc
+++ b/src/ui/map3D/ObstacleGroupNode.cc
@@ -57,23 +57,13 @@ ObstacleGroupNode::clear(void)
 }
 
 void
-ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
+ObstacleGroupNode::update(double robotX, double robotY, double robotZ,
+                          const px::ObstacleList& obstacleList)
 {
-    if (!uas || frame == MAV_FRAME_GLOBAL)
-    {
-        return;
-    }
-
-    double robotX = uas->getLocalX();
-    double robotY = uas->getLocalY();
-    double robotZ = uas->getLocalZ();
-
     clear();
 
     osg::ref_ptr<osg::Geode> geode = new osg::Geode;
 
-    px::ObstacleList obstacleList = uas->getObstacleList();
-
     for (int i = 0; i < obstacleList.obstacles_size(); ++i)
     {
         const px::Obstacle& obs = obstacleList.obstacles(i);
diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h
index 36f1892a614d07974cdb99d3a364caf8f57c92b8..8a0715ebe566fa67ff97e4e298c724e1b1cdd6df 100644
--- a/src/ui/map3D/ObstacleGroupNode.h
+++ b/src/ui/map3D/ObstacleGroupNode.h
@@ -43,7 +43,8 @@ public:
 
     void init(void);
     void clear(void);
-    void update(MAV_FRAME frame, UASInterface* uas);
+    void update(double robotX, double robotY, double robotZ,
+                const px::ObstacleList& obstacleList);
 };
 
 #endif // OBSTACLEGROUPNODE_H
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index 516058d330fa6b964bf695a498481dfcd1f1896f..f252420f6a2f5c79563895eb80b51916133fe174 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -53,7 +53,7 @@
 Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     : Q3DWidget(parent)
     , uas(NULL)
-    , kMessageTimeout(2.0)
+    , kMessageTimeout(4.0)
     , mode(DEFAULT_MODE)
     , selectedWpIndex(-1)
     , displayLocalGrid(false)
@@ -791,7 +791,7 @@ Pixhawk3DWidget::display(void)
 
     if (displayObstacleList)
     {
-        updateObstacles();
+        updateObstacles(robotX, robotY, robotZ);
     }
 
     if (displayPath)
@@ -1534,10 +1534,12 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
 void
 Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 {
-    px::RGBDImage rgbdImage = uas->getRGBDImage();
-    px::PointCloudXYZRGB pointCloud = uas->getPointCloud();
+    qreal receivedRGBDImageTimestamp, receivedPointCloudTimestamp;
+    px::RGBDImage rgbdImage = uas->getRGBDImage(receivedRGBDImageTimestamp);
+    px::PointCloudXYZRGB pointCloud = uas->getPointCloud(receivedPointCloudTimestamp);
 
-    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0)
+    if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0 &&
+        QGC::groundTimeSeconds() - receivedRGBDImageTimestamp < kMessageTimeout)
     {
         rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1,
                            GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE,
@@ -1576,9 +1578,15 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
     }
 
     osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
-
     osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
     osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
+
+    if (QGC::groundTimeSeconds() - receivedPointCloudTimestamp > kMessageTimeout)
+    {
+        geometry->removePrimitiveSet(0, geometry->getNumPrimitiveSets());
+        return;
+    }
+
     for (int i = 0; i < pointCloud.points_size(); ++i)
     {
         const px::PointCloudXYZRGB_PointXYZRGB& p = pointCloud.points(i);
@@ -1625,11 +1633,20 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateObstacles(void)
+Pixhawk3DWidget::updateObstacles(double robotX, double robotY, double robotZ)
 {
-    if (QGC::groundTimeSeconds() - uas->getObstacleList().header().timestamp() < kMessageTimeout)
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        obstacleGroupNode->clear();
+        return;
+    }
+
+    qreal receivedTimestamp;
+    px::ObstacleList obstacleList = uas->getObstacleList(receivedTimestamp);
+
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
     {
-        obstacleGroupNode->update(frame, uas);
+        obstacleGroupNode->update(robotX, robotY, robotZ, obstacleList);
     }
     else
     {
@@ -1640,7 +1657,8 @@ Pixhawk3DWidget::updateObstacles(void)
 void
 Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
 {
-    px::Path path = uas->getPath();
+    qreal receivedTimestamp;
+    px::Path path = uas->getPath(receivedTimestamp);
 
     osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry();
     osg::DrawArrays* drawArrays = reinterpret_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
@@ -1655,7 +1673,7 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
 
     osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
 
-    if (QGC::groundTimeSeconds() - path.header().timestamp() < kMessageTimeout)
+    if (QGC::groundTimeSeconds() - receivedTimestamp < kMessageTimeout)
     {
         // find path length
         float length = 0.0f;
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index 8adfb1fa59b1d506aa109f1950dc2c0529683619..c5bde68114694d65057fd67c391334f58dc7945a 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -127,7 +127,7 @@ private:
     void updateTarget(double robotX, double robotY, double robotZ);
 #ifdef QGC_PROTOBUF_ENABLED
     void updateRGBD(double robotX, double robotY, double robotZ);
-    void updateObstacles(void);
+    void updateObstacles(double robotX, double robotY, double robotZ);
     void updatePath(double robotX, double robotY, double robotZ);
 #endif