From c3f4386a52d05430ce8e5b709a77b3e41fc5f89a Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Thu, 12 Apr 2018 12:22:55 +0800
Subject: [PATCH] Complete enable motion datas

---
 samples/device/camera.cc | 19 +++++++++++++++++++
 src/internal/motions.cc  | 11 +++++++++--
 src/internal/motions.h   |  3 +++
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/samples/device/camera.cc b/samples/device/camera.cc
index 8dea9d9..7f3784c 100644
--- a/samples/device/camera.cc
+++ b/samples/device/camera.cc
@@ -100,10 +100,13 @@ int main(int argc, char *argv[]) {
             << ", temperature: " << data.imu->temperature;
   });
 
+  // Enable this will cache the motion datas until you get them.
+  device->EnableMotionDatas();
   device->Start(Source::ALL);
 
   cv::namedWindow("frame");
 
+  std::size_t motion_count = 0;
   auto &&time_beg = times::now();
   while (true) {
     device->WaitForStreams();
@@ -111,6 +114,20 @@ int main(int argc, char *argv[]) {
     device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT);
     device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT);
 
+    auto &&motion_datas = device->GetMotionDatas();
+    motion_count += motion_datas.size();
+    for (auto &&data : motion_datas) {
+      LOG(INFO) << "  frame_id: " << data.imu->frame_id
+                << ", timestamp: " << data.imu->timestamp
+                << ", accel_x: " << data.imu->accel[0]
+                << ", accel_y: " << data.imu->accel[1]
+                << ", accel_z: " << data.imu->accel[2]
+                << ", gyro_x: " << data.imu->gyro[0]
+                << ", gyro_y: " << data.imu->gyro[1]
+                << ", gyro_z: " << data.imu->gyro[2]
+                << ", temperature: " << data.imu->temperature;
+    }
+
     cv::Mat left_img(
         left_data.frame->height(), left_data.frame->width(), CV_8UC1,
         left_data.frame->data());
@@ -142,5 +159,7 @@ int main(int argc, char *argv[]) {
             << ", fps: " << (1000.f * right_count / elapsed_ms);
   LOG(INFO) << "Imu count: " << imu_count
             << ", hz: " << (1000.f * imu_count / elapsed_ms);
+  // LOG(INFO) << "Motion count: " << motion_count
+  //           << ", hz: " << (1000.f * motion_count / elapsed_ms);
   return 0;
 }
diff --git a/src/internal/motions.cc b/src/internal/motions.cc
index 091d14f..e8da952 100644
--- a/src/internal/motions.cc
+++ b/src/internal/motions.cc
@@ -45,8 +45,12 @@ void Motions::StartMotionTracking() {
         imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
         imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
         imu->temperature = seg.temperature / 326.8f + 25;
+
+        std::lock_guard<std::mutex> _(mtx_datas_);
+        motion_data_t data = {imu};
+        motion_datas_.push_back(data);
         if (motion_callback_) {
-          motion_callback_({imu});
+          motion_callback_(data);
         }
       }
     });
@@ -77,7 +81,10 @@ Motions::motion_datas_t Motions::GetMotionDatas() {
     LOG(FATAL) << "Must enable motion datas before getting them, or you set "
                   "motion callback instead";
   }
-  return motion_datas_;
+  std::lock_guard<std::mutex> _(mtx_datas_);
+  motion_datas_t datas = motion_datas_;
+  motion_datas_.clear();
+  return datas;
 }
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/internal/motions.h b/src/internal/motions.h
index 4e3d715..607f8a8 100644
--- a/src/internal/motions.h
+++ b/src/internal/motions.h
@@ -3,6 +3,7 @@
 #pragma once
 
 #include <memory>
+#include <mutex>
 #include <vector>
 
 #include "mynteye/mynteye.h"
@@ -41,6 +42,8 @@ class Motions {
   std::size_t motion_datas_max_size;
 
   bool is_imu_tracking;
+
+  std::mutex mtx_datas_;
 };
 
 MYNTEYE_END_NAMESPACE