Merge branch 'feature/one-thread-read' into develop

src/device/device.cc

@@ -434,6 +434,7 @@ void Device::StartVideoStreaming() {
             }
           }
           continuation();
+          OnStereoStreamUpdate();
           // VLOG(2) << "Stereo video callback cost "
           //     << times::count<times::milliseconds>(times::now() - time_beg)
           //     << " ms";
@@ -465,7 +466,7 @@ void Device::StartMotionTracking() {
   }
   motions_->SetMotionCallback(
       std::bind(&Device::CallbackMotionData, this, std::placeholders::_1));
-  motions_->StartMotionTracking();
+  // motions_->StartMotionTracking();
   motion_tracking_ = true;
 }
 
@@ -474,10 +475,12 @@ void Device::StopMotionTracking() {
     LOG(WARNING) << "Cannot stop motion tracking without first starting it";
     return;
   }
-  motions_->StopMotionTracking();
+  // motions_->StopMotionTracking();
   motion_tracking_ = false;
 }
 
+void Device::OnStereoStreamUpdate() {}
+
 void Device::ReadAllInfos() {
   device_info_ = std::make_shared<DeviceInfo>();
 

src/device/device.h

@@ -239,6 +239,18 @@ class MYNTEYE_API Device {
     return device_;
   }
 
+  std::shared_ptr<Streams> streams() const {
+    return streams_;
+  }
+
+  std::shared_ptr<Channels> channels() const {
+    return channels_;
+  }
+
+  std::shared_ptr<Motions> motions() const {
+    return motions_;
+  }
+
   const StreamRequest &GetStreamRequest(const Capabilities &capability);
 
   virtual void StartVideoStreaming();
@@ -247,6 +259,8 @@ class MYNTEYE_API Device {
   virtual void StartMotionTracking();
   virtual void StopMotionTracking();
 
+  virtual void OnStereoStreamUpdate();
+
   virtual std::vector<Stream> GetKeyStreams() const = 0;
 
   bool video_streaming_;
@@ -284,10 +298,6 @@ class MYNTEYE_API Device {
   void CallbackPushedStreamData(const Stream &stream);
   void CallbackMotionData(const device::MotionData &data);
 
-  std::shared_ptr<Channels> channels() {
-    return channels_;
-  }
-
   friend API;
   friend tools::DeviceWriter;
 };

src/device/device_s.cc

@@ -15,6 +15,8 @@
 
 #include <glog/logging.h>
 
+#include "internal/motions.h"
+
 MYNTEYE_BEGIN_NAMESPACE
 
 StandardDevice::StandardDevice(std::shared_ptr<uvc::device> device)
@@ -30,4 +32,11 @@ std::vector<Stream> StandardDevice::GetKeyStreams() const {
   return {Stream::LEFT, Stream::RIGHT};
 }
 
+void StandardDevice::OnStereoStreamUpdate() {
+  if (motion_tracking_) {
+    auto &&motions = this->motions();
+    motions->DoMotionTrack();
+  }
+}
+
 MYNTEYE_END_NAMESPACE

src/device/device_s.h

@@ -28,6 +28,8 @@ class StandardDevice : public Device {
   virtual ~StandardDevice();
 
   std::vector<Stream> GetKeyStreams() const override;
+
+  void OnStereoStreamUpdate() override;
 };
 
 MYNTEYE_END_NAMESPACE

src/internal/channels.cc

@@ -31,7 +31,12 @@ MYNTEYE_BEGIN_NAMESPACE
 
 namespace {
 
-const uvc::xu mynteye_xu = {3, 2, {0x947a6d9f, 0x8a2f, 0x418d, {0x85, 0x9e, 0x6c, 0x9a, 0xa0, 0x38, 0x10, 0x14}}};
+const uvc::xu mynteye_xu = {3,
+                            2,
+                            {0x947a6d9f,
+                             0x8a2f,
+                             0x418d,
+                             {0x85, 0x9e, 0x6c, 0x9a, 0xa0, 0x38, 0x10, 0x14}}};
 
 int XuCamCtrlId(Option option) {
   switch (option) {
@@ -276,6 +281,47 @@ void Channels::SetImuCallback(imu_callback_t callback) {
   imu_callback_ = callback;
 }
 
+void Channels::DoImuTrack() {
+  static ImuReqPacket req_packet{0};
+  static ImuResPacket res_packet;
+
+  req_packet.serial_number = imu_sn_;
+  if (!XuImuWrite(req_packet)) {
+    return;
+  }
+
+  if (!XuImuRead(&res_packet)) {
+    return;
+  }
+
+  if (res_packet.packets.size() == 0) {
+    return;
+  }
+
+  VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
+    std::size_t n = 0;
+    for (auto &&packet : res_packet.packets) {
+      n += packet.count;
+    }
+    return n;
+  }();
+
+  auto &&sn = res_packet.packets.back().serial_number;
+  if (imu_sn_ == sn) {
+    VLOG(2) << "New imu not ready, dropped";
+    return;
+  }
+  imu_sn_ = sn;
+
+  if (imu_callback_) {
+    for (auto &&packet : res_packet.packets) {
+      imu_callback_(packet);
+    }
+  }
+
+  res_packet.packets.clear();
+}
+
 void Channels::StartImuTracking(imu_callback_t callback) {
   if (is_imu_tracking_) {
     LOG(WARNING) << "Start imu tracking failed, is tracking already";
@@ -287,8 +333,6 @@ void Channels::StartImuTracking(imu_callback_t callback) {
   is_imu_tracking_ = true;
   imu_track_thread_ = std::thread([this]() {
     imu_sn_ = 0;
-    ImuReqPacket req_packet{imu_sn_};
-    ImuResPacket res_packet;
     auto sleep = [](const times::system_clock::time_point &time_beg) {
       auto &&time_elapsed_ms =
           times::count<times::milliseconds>(times::now() - time_beg);
@@ -301,48 +345,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
     };
     while (!imu_track_stop_) {
       auto &&time_beg = times::now();
-
+      DoImuTrack();
-      req_packet.serial_number = imu_sn_;
-      if (!XuImuWrite(req_packet)) {
-        sleep(time_beg);
-        continue;
-      }
-
-      if (!XuImuRead(&res_packet)) {
-        sleep(time_beg);
-        continue;
-      }
-
-      if (res_packet.packets.size() == 0) {
-        sleep(time_beg);
-        continue;
-      }
-
-      VLOG(2) << "Imu req sn: " << imu_sn_
-              << ", res count: " << [&res_packet]() {
-                   std::size_t n = 0;
-                   for (auto &&packet : res_packet.packets) {
-                     n += packet.count;
-                   }
-                   return n;
-                 }();
-
-      auto &&sn = res_packet.packets.back().serial_number;
-      if (imu_sn_ == sn) {
-        VLOG(2) << "New imu not ready, dropped";
-        sleep(time_beg);
-        continue;
-      }
-      imu_sn_ = sn;
-
-      if (imu_callback_) {
-        for (auto &&packet : res_packet.packets) {
-          imu_callback_(packet);
-        }
-      }
-
-      res_packet.packets.clear();
-
       sleep(time_beg);
     }
   });
@@ -870,7 +873,8 @@ bool Channels::PuControlQuery(
 }
 
 bool Channels::XuControlRange(
-    channel_t channel, uint8_t id, int32_t *min, int32_t *max, int32_t *def) const {
+    channel_t channel, uint8_t id, int32_t *min, int32_t *max,
+    int32_t *def) const {
   return XuControlRange(mynteye_xu, channel, id, min, max, def);
 }
 
@@ -1016,7 +1020,8 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const {
   int id = XuCamCtrlId(option);
 
   int32_t min = 0, max = 0, def = 0;
-  if (!XuControlRange(CHANNEL_CAM_CTRL, static_cast<std::uint8_t>(id), &min, &max, &def)) {
+  if (!XuControlRange(
+          CHANNEL_CAM_CTRL, static_cast<std::uint8_t>(id), &min, &max, &def)) {
     LOG(WARNING) << "Get XuControlInfo of " << option << " failed";
   }
   return {min, max, def};

src/internal/channels.h

@@ -95,6 +95,8 @@ class MYNTEYE_API Channels {
   bool RunControlAction(const Option &option) const;
 
   void SetImuCallback(imu_callback_t callback);
+  void DoImuTrack();
+
   void StartImuTracking(imu_callback_t callback = nullptr);
   void StopImuTracking();
 
@@ -111,10 +113,11 @@ class MYNTEYE_API Channels {
   bool PuControlQuery(Option option, uvc::pu_query query, int32_t *value) const;
 
   bool XuControlRange(
-      channel_t channel, uint8_t id, int32_t *min, int32_t *max, int32_t *def) const;
+      channel_t channel, uint8_t id, int32_t *min, int32_t *max,
-  bool XuControlRange(
-      const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min, int32_t *max,
       int32_t *def) const;
+  bool XuControlRange(
+      const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min,
+      int32_t *max, int32_t *def) const;
 
   bool XuControlQuery(
       channel_t channel, uvc::xu_query query, uint16_t size,

src/internal/motions.cc

@@ -34,11 +34,8 @@ Motions::~Motions() {
 
 void Motions::SetMotionCallback(motion_callback_t callback) {
   motion_callback_ = callback;
-}
+  if (motion_callback_) {
-
+    channels_->SetImuCallback([this](const ImuPacket &packet) {
-void Motions::StartMotionTracking() {
-  if (!is_imu_tracking) {
-    channels_->StartImuTracking([this](const ImuPacket &packet) {
       if (!motion_callback_ && !motion_datas_enabled_) {
         LOG(WARNING) << "";
         return;
@@ -62,11 +59,22 @@ void Motions::StartMotionTracking() {
         std::lock_guard<std::mutex> _(mtx_datas_);
         motion_data_t data = {imu};
         motion_datas_.push_back(data);
-        if (motion_callback_) {
+
         motion_callback_(data);
       }
-      }
     });
+  } else {
+    channels_->SetImuCallback(nullptr);
+  }
+}
+
+void Motions::DoMotionTrack() {
+  channels_->DoImuTrack();
+}
+
+void Motions::StartMotionTracking() {
+  if (!is_imu_tracking) {
+    channels_->StartImuTracking();
     is_imu_tracking = true;
   } else {
     LOG(WARNING) << "Imu is tracking already";

src/internal/motions.h

@@ -37,6 +37,7 @@ class Motions {
   ~Motions();
 
   void SetMotionCallback(motion_callback_t callback);
+  void DoMotionTrack();
 
   void StartMotionTracking();
   void StopMotionTracking();