test(timestamp): s2 stamp use normal logic.

src/mynteye/device/channel/channels.cc

@@ -141,6 +141,7 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
     adapter_(adapter),
     is_imu_tracking_(false),
     is_imu_proto2_(false),
+    is_s2_(false),
     enable_imu_correspondence(false),
     imu_track_stop_(false),
     imu_sn_(0),
@@ -422,7 +423,11 @@ void Channels::DoImuTrack() {
   if (IsImuProtocol2()) {
     return DoImuTrack2();
   } else {
+    if (IsS2()) {
       return DoImuTrack1();
+    } else {
+      return DoImuTrack1WithTimeLimmitFix();
+    }
   }
 }
 
@@ -462,6 +467,50 @@ void Channels::DoImuTrack1() {
   }
   imu_sn_ = sn;
 
+  if (imu_callback_) {
+    for (auto &packet : res_packet.packets) {
+      imu_callback_(to_pak2(packet, accel_range, gyro_range));
+    }
+  }
+  res_packet.packets.clear();
+}
+
+void Channels::DoImuTrack1WithTimeLimmitFix() {
+  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;
+  }
+
+  if (res_packet.packets.back().count == 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) {
       auto pak2_tmp = to_pak2(packet, accel_range, gyro_range);
@@ -522,7 +571,6 @@ void Channels::DoImuTrack2() {
   imu_sn_ = sn;
   if (imu_callback_) {
     for (auto &packet : res_packet.packets) {
-      CheckTimeStampLimmit(packet);
       imu_callback_(packet);
     }
   }
@@ -559,11 +607,19 @@ void Channels::StartImuTracking(imu_callback_t callback) {
         sleep(time_beg);
       }
     } else {
+      if (IsS2()) {
         while (!imu_track_stop_) {
           auto &&time_beg = times::now();
           DoImuTrack1();
           sleep(time_beg);
         }
+      } else {
+        while (!imu_track_stop_) {
+          auto &&time_beg = times::now();
+          DoImuTrack1WithTimeLimmitFix();
+          sleep(time_beg);
+        }
+      }
     }
   });
 }
@@ -643,6 +699,7 @@ bool Channels::GetFiles(
           is_imu_proto2_ = dev_info_ &&
               dev_info_->spec_version >= Version(1, 3) &&
               strstr(dev_info_->name.c_str(), "S2") != nullptr;
+          is_s2_ = strstr(dev_info_->name.c_str(), "S2") != nullptr;
         } break;
         case FID_IMG_PARAMS: {
           if (file_size > 0) {

src/mynteye/device/channel/channels.h

@@ -82,6 +82,7 @@ class MYNTEYE_API Channels {
   void SetImuCallback(imu_callback_t callback);
   void DoImuTrack();
   void DoImuTrack1();
+  void DoImuTrack1WithTimeLimmitFix();
   void DoImuTrack2();
 
   void StartImuTracking(imu_callback_t callback = nullptr);
@@ -92,6 +93,7 @@ class MYNTEYE_API Channels {
   bool SetFiles(
       device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
   inline bool IsImuProtocol2() const { return is_imu_proto2_; }
+  inline bool IsS2() const { return is_s2_; }
   inline void EnableImuCorrespondence(bool is_enable) {
     enable_imu_correspondence = is_enable;
   }
@@ -145,6 +147,7 @@ class MYNTEYE_API Channels {
 
   bool is_imu_tracking_;
   bool is_imu_proto2_;
+  bool is_s2_;
   bool enable_imu_correspondence;
   std::thread imu_track_thread_;
   volatile bool imu_track_stop_;

src/mynteye/device/device.cc

@@ -534,6 +534,34 @@ void Device::StartVideoStreaming() {
     auto &&stream_request = GetStreamRequest(stream_cap);
     streams_->ConfigStream(stream_cap, stream_request);
 
+    if (strstr(this->device_info_->name.c_str(), "S2") == nullptr) {
+      uvc::set_device_mode(
+          *device_, stream_request.width, stream_request.height,
+          static_cast<int>(stream_request.format), stream_request.fps,
+          [this, stream_cap](
+              const void *data, std::function<void()> continuation) {
+            // drop the first stereo stream data
+            static std::uint8_t drop_count = 1;
+            if (drop_count > 0) {
+              --drop_count;
+              continuation();
+              return;
+            }
+            // auto &&time_beg = times::now();
+            {
+              std::lock_guard<std::mutex> _(mtx_streams_);
+              if (streams_->PushStreamS1(stream_cap, data)) {
+                CallbackPushedStreamData(Stream::LEFT);
+                CallbackPushedStreamData(Stream::RIGHT);
+              }
+            }
+            continuation();
+            OnStereoStreamUpdate();
+            // VLOG(2) << "Stereo video callback cost "
+            //     << times::count<times::milliseconds>(times::now() - time_beg)
+            //     << " ms";
+          });
+    } else {
       uvc::set_device_mode(
           *device_, stream_request.width, stream_request.height,
           static_cast<int>(stream_request.format), stream_request.fps,
@@ -560,6 +588,7 @@ void Device::StartVideoStreaming() {
             //     << times::count<times::milliseconds>(times::now() - time_beg)
             //     << " ms";
           });
+    }
   } else {
     LOG(FATAL) << "Not any stream capabilities are supported by this device";
   }

src/mynteye/device/standard2/channels_adapter_s2.cc

@@ -15,6 +15,8 @@
 
 #include "mynteye/logger.h"
 
+#define DEBUG_TIME_LIMIT true
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace {
@@ -43,6 +45,9 @@ struct ImuData {
     timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
                   (*(data + 10) << 8) | *(data + 11);
     timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
+#if DEBUG_TIME_LIMIT
+    timestamp += 4200000000;
+#endif
     flag = *(data + 12);
     temperature = (*(data + 13) << 8) | *(data + 14);
     accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);

src/mynteye/device/standard2/streams_adapter_s2.cc

@@ -18,6 +18,8 @@
 #include "mynteye/logger.h"
 #include "mynteye/device/types.h"
 
+#define DEBUG_TIME_LIMIT true
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace {
@@ -51,6 +53,9 @@ struct ImagePacket {
     timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
                   (*(data + 10) << 8) | *(data + 11);
     timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
+#if DEBUG_TIME_LIMIT
+    timestamp += 4200000000;
+#endif
     exposure_time = (*(data + 12) << 8) | *(data + 13);
     checksum = *(data + 14);
   }

src/mynteye/device/streams.cc

@@ -66,6 +66,49 @@ void Streams::CheckTimeStampLimmit(std::shared_ptr<ImgData> img) {
 }
 
 bool Streams::PushStream(const Capabilities &capability, const void *data) {
+  if (!HasStreamConfigRequest(capability)) {
+    LOG(FATAL) << "Cannot push stream without stream config request";
+  }
+  std::unique_lock<std::mutex> lock(mtx_);
+  auto &&request = GetStreamConfigRequest(capability);
+  bool pushed = false;
+  switch (capability) {
+    case Capabilities::STEREO:
+    case Capabilities::STEREO_COLOR: {
+      // alloc left
+      AllocStreamData(capability, Stream::LEFT, request);
+      auto &&left_data = stream_datas_map_[Stream::LEFT].back();
+      // unpack img data
+      if (unpack_img_data_map_[Stream::LEFT](
+              data, request, left_data.img.get())) {
+        left_data.frame_id = left_data.img->frame_id;
+        // alloc right
+        AllocStreamData(capability, Stream::RIGHT, request);
+        auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
+        *right_data.img = *left_data.img;
+        right_data.frame_id = left_data.img->frame_id;
+        // unpack frame
+        unpack_img_pixels_map_[Stream::LEFT](
+            data, request, left_data.frame.get());
+        unpack_img_pixels_map_[Stream::RIGHT](
+            data, request, right_data.frame.get());
+        pushed = true;
+      } else {
+        // discard left
+        DiscardStreamData(Stream::LEFT);
+        VLOG(2) << "Image packet is unaccepted, frame dropped";
+        pushed = false;
+      }
+    } break;
+    default:
+      LOG(FATAL) << "Not supported " << capability << " now";
+  }
+  if (HasKeyStreamDatas())
+    cv_.notify_one();
+  return pushed;
+}
+
+bool Streams::PushStreamS1(const Capabilities &capability, const void *data) {
   if (!HasStreamConfigRequest(capability)) {
     LOG(FATAL) << "Cannot push stream without stream config request";
   }

src/mynteye/device/streams.h

@@ -49,6 +49,7 @@ class Streams {
       const Capabilities &capability, const StreamRequest &request);
 
   bool PushStream(const Capabilities &capability, const void *data);
+  bool PushStreamS1(const Capabilities &capability, const void *data);
 
   void WaitForStreams();