Get stream & motion datas in one thread

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();
-
-      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();
-
+      DoImuTrack();
       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;
-  bool XuControlRange(
-      const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min, int32_t *max,
+      channel_t channel, 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;
-}
-
-void Motions::StartMotionTracking() {
-  if (!is_imu_tracking) {
-    channels_->StartImuTracking([this](const ImuPacket &packet) {
+  if (motion_callback_) {
+    channels_->SetImuCallback([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);
-        }
+
+        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();