From 998e753cbff5aa4c993ee4128d4c719fca08c02b Mon Sep 17 00:00:00 2001
From: Lionel Heng <hengli@student.ethz.ch>
Date: Thu, 23 Feb 2012 21:00:16 +0100
Subject: [PATCH] Added timeout to overlay visualization.

---
 src/ui/map3D/GLOverlayGeode.cc  | 13 +++++++++++++
 src/ui/map3D/GLOverlayGeode.h   |  5 +++++
 src/ui/map3D/Pixhawk3DWidget.cc | 15 +++++++++++----
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/src/ui/map3D/GLOverlayGeode.cc b/src/ui/map3D/GLOverlayGeode.cc
index b92556c0d..fdf183788 100644
--- a/src/ui/map3D/GLOverlayGeode.cc
+++ b/src/ui/map3D/GLOverlayGeode.cc
@@ -2,6 +2,7 @@
 
 GLOverlayGeode::GLOverlayGeode()
  : mDrawable(new GLOverlayDrawable)
+ , mMessageTimestamp(0.0)
 {
     addDrawable(mDrawable);
 }
@@ -21,6 +22,18 @@ GLOverlayGeode::coordinateFrameType(void) const
     return mCoordinateFrameType;
 }
 
+void
+GLOverlayGeode::setMessageTimestamp(qreal timestamp)
+{
+    mMessageTimestamp = timestamp;
+}
+
+qreal
+GLOverlayGeode::messageTimestamp(void) const
+{
+    return mMessageTimestamp;
+}
+
 GLOverlayGeode::GLOverlayDrawable::GLOverlayDrawable()
 {
     setUseDisplayList(true);
diff --git a/src/ui/map3D/GLOverlayGeode.h b/src/ui/map3D/GLOverlayGeode.h
index d84647ab0..d70227c8d 100644
--- a/src/ui/map3D/GLOverlayGeode.h
+++ b/src/ui/map3D/GLOverlayGeode.h
@@ -3,6 +3,7 @@
 
 #include <mavlink_protobuf_manager.hpp>
 #include <osg/Geode>
+#include <QtGlobal>
 
 class GLOverlayGeode : public osg::Geode
 {
@@ -13,6 +14,9 @@ public:
 
     px::GLOverlay::CoordinateFrameType coordinateFrameType(void) const;
 
+    void setMessageTimestamp(qreal timestamp);
+    qreal messageTimestamp(void) const;
+
 private:
     class GLOverlayDrawable : public osg::Drawable
     {
@@ -39,6 +43,7 @@ private:
 
     osg::ref_ptr<GLOverlayDrawable> mDrawable;
     px::GLOverlay::CoordinateFrameType mCoordinateFrameType;
+    qreal mMessageTimestamp;
 };
 
 #endif // GLOVERLAYGEODE_H
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index 081ede5dc..f6be32e30 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -563,7 +563,8 @@ Pixhawk3DWidget::addOverlay(UASInterface *uas)
 
     SystemContainer& systemData = mSystemContainerMap[systemId];
 
-    px::GLOverlay overlay = uas->getOverlay();
+    qreal receivedTimestamp;
+    px::GLOverlay overlay = uas->getOverlay(receivedTimestamp);
 
     QString overlayName = QString::fromStdString(overlay.name());
 
@@ -580,7 +581,9 @@ Pixhawk3DWidget::addOverlay(UASInterface *uas)
         emit overlayCreatedSignal(systemId, overlayName);
     }
 
-    systemData.overlayNodeMap()[overlayName]->setOverlay(overlay);
+    osg::ref_ptr<GLOverlayGeode>& overlayNode = systemData.overlayNodeMap()[overlayName];
+    overlayNode->setOverlay(overlay);
+    overlayNode->setMessageTimestamp(receivedTimestamp);
 }
 #endif
 
@@ -953,11 +956,15 @@ Pixhawk3DWidget::update(void)
             bool displayOverlay = systemViewParams->displayOverlay().value(itOverlay.key());
 
             bool visible;
-            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::GLOBAL) && displayOverlay;
+            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::GLOBAL) &&
+                      displayOverlay &&
+                      (QGC::groundTimeSeconds() - overlayNode->messageTimestamp() < kMessageTimeout);
 
             allocentricMap->setChildValue(overlayNode, visible);
 
-            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::LOCAL) && displayOverlay;
+            visible = (overlayNode->coordinateFrameType() == px::GLOverlay::LOCAL) &&
+                      displayOverlay &&
+                      (QGC::groundTimeSeconds() - overlayNode->messageTimestamp() < kMessageTimeout);;
 
             rollingMap->setChildValue(overlayNode, visible);
         }
-- 
2.22.0