Improved OpenNI driver

* OpenNI wants the timestamp in microseconds so we have to divide the 60MHz clock timestamp to get the correct value
* Input timestamp is a uint32_t, thus overflowing after ~70s. The timestamp now is stored in a uint64_t value, overflows are now handled and the returned time is monotonically increasing.

(cherry picked from commit 85a9980)
Signed-off-by: Benn Snyder <[email protected]>

Author: tcavallari

OpenNI2-FreenectDriver/src/VideoStream.hpp

@@ -11,6 +11,7 @@ namespace FreenectDriver
   {
   private:
     unsigned int frame_id; // number each frame
+    uint64_t prev_timestamp; // timestamp of the last processed frame
 
     virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0;
     virtual void populateFrame(void* data, OniFrame* frame) const = 0;
@@ -26,6 +27,7 @@ namespace FreenectDriver
   public:
     VideoStream(Freenect::FreenectDevice* device) :
       frame_id(1),
+      prev_timestamp(0),
       device(device),
       mirroring(false)
       {
@@ -42,11 +44,24 @@ namespace FreenectDriver
 
       OniFrame* frame = getServices().acquireFrame();
       frame->frameIndex = frame_id++;
-      frame->timestamp = timestamp;
       frame->videoMode = video_mode;
       frame->width = video_mode.resolutionX;
       frame->height = video_mode.resolutionY;
 
+      // Handle overflow, input timestamp comes from a 60MHz clock and overflows
+      // in ~70s
+      if (timestamp < prev_timestamp)
+      {
+        uint32_t prev_int = static_cast<uint32_t>(prev_timestamp);
+        uint64_t temp_delta = std::abs<int64_t>(timestamp - prev_int);
+        prev_timestamp += temp_delta;
+      } else {
+        prev_timestamp = timestamp;
+      }
+
+      // OpenNI wants the value in microseconds
+      frame->timestamp = prev_timestamp / 60;
+
       populateFrame(data, frame);
       raiseNewFrame(frame);
       getServices().releaseFrame(frame);