fix(timestamp): add timestamp compensate for overflow value in img stream.

src/mynteye/device/channel/channels.cc

@@ -28,6 +28,9 @@
 
 #define IMU_TRACK_PERIOD 25  // ms
 
+// for time limmit debug
+#define DEBUG_TIME_LIMIT true
+
 MYNTEYE_BEGIN_NAMESPACE
 
 mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1,
@@ -462,14 +465,14 @@ void Channels::DoImuTrack1() {
   if (imu_callback_) {
     for (auto &packet : res_packet.packets) {
       auto pak2_tmp = to_pak2(packet, accel_range, gyro_range);
-      checkTimeStampLimmit(pak2_tmp);
+      CheckTimeStampLimmit(pak2_tmp);
       imu_callback_(pak2_tmp);
     }
   }
   res_packet.packets.clear();
 }
 
-void Channels::checkTimeStampLimmit(mynteye::ImuPacket2 &packet) {
+void Channels::CheckTimeStampLimmit(mynteye::ImuPacket2 &packet) {
   for (auto &segment2 : packet.segments) {
     segment2.timestamp += (timestamp_compensate_ * 42949672960);
     // the timestamp nearly 8 imu frame before limmit
@@ -481,13 +484,7 @@ void Channels::checkTimeStampLimmit(mynteye::ImuPacket2 &packet) {
     }
     if (is_nearly_before_timestamp_limmit_ > 0) {
       is_nearly_before_timestamp_limmit_--;
-#if DEBUG_TIME_LIMIT
-      LOG(WARNING) << "is_nearly_before_timestamp_limmit_--;";
-#endif
       if (abs(current_datum_ - segment2.timestamp) > (u_int64_t)(42949672960/2)) {  // NOLINT
-#if DEBUG_TIME_LIMIT
-        LOG(WARNING) << "abs(current_datum_ - segment2.timestamp) > 42949672960/2";  // NOLINT
-#endif
         segment2.timestamp -= 42949672960;
       }
     }
@@ -525,7 +522,7 @@ void Channels::DoImuTrack2() {
   imu_sn_ = sn;
   if (imu_callback_) {
     for (auto &packet : res_packet.packets) {
-      checkTimeStampLimmit(packet);
+      CheckTimeStampLimmit(packet);
       imu_callback_(packet);
     }
   }

src/mynteye/device/channel/channels.h

@@ -131,7 +131,7 @@ class MYNTEYE_API Channels {
   bool XuImuRead(ImuResPacket2 *res) const;
 
   bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
-  void checkTimeStampLimmit(mynteye::ImuPacket2 &packet);
+  void CheckTimeStampLimmit(mynteye::ImuPacket2 &packet);  // NOLINT
 
   control_info_t PuControlInfo(Option option) const;
   control_info_t XuControlInfo(Option option) const;

src/mynteye/device/channel/def.h

@@ -21,9 +21,6 @@
 
 #include "mynteye/mynteye.h"
 
-// for time limmit debug
-#define DEBUG_TIME_LIMIT true
-
 MYNTEYE_BEGIN_NAMESPACE
 
 typedef enum Channel {

src/mynteye/device/standard/channels_adapter_s.cc

@@ -15,6 +15,9 @@
 
 #include "mynteye/logger.h"
 
+// for time limmit debug
+#define DEBUG_TIME_LIMIT false
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace {

src/mynteye/device/standard/streams_adapter_s.cc

@@ -19,6 +19,7 @@
 #include "mynteye/device/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
+#define DEBUG_TIME_LIMIT false
 
 namespace {
 
@@ -29,7 +30,7 @@ struct ImagePacket {
   std::uint8_t header;
   std::uint8_t size;
   std::uint16_t frame_id;
-  std::uint32_t timestamp;
+  std::uint64_t timestamp;
   std::uint16_t exposure_time;
   std::uint8_t checksum;
 
@@ -42,8 +43,15 @@ struct ImagePacket {
     header = *data;
     size = *(data + 1);
     frame_id = (*(data + 2) << 8) | *(data + 3);
+#if DEBUG_TIME_LIMIT
+    uint32_t timestamp_test = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) | // NOLINT
+                *(data + 7);
+    timestamp_test += (std::uint32_t)4290000000;
+    timestamp = timestamp_test;
+#else
     timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
                 *(data + 7);
+#endif
     exposure_time = (*(data + 8) << 8) | *(data + 9);
     checksum = *(data + 10);
   }
@@ -59,7 +67,7 @@ bool unpack_stereo_img_data(
       request.width * request.height * bytes_per_pixel(request.format);
   auto data_end = data_new + data_n;
 
-  std::size_t packet_n = sizeof(ImagePacket);
+  std::size_t packet_n = sizeof(ImagePacket) - 4;
   std::vector<std::uint8_t> packet(packet_n);
   std::reverse_copy(data_end - packet_n, data_end, packet.begin());
 

src/mynteye/device/streams.cc

@@ -16,6 +16,7 @@
 #include <algorithm>
 #include <chrono>
 #include <stdexcept>
+#include <utility>
 
 #include "mynteye/logger.h"
 #include "mynteye/device/types.h"
@@ -26,7 +27,10 @@ Streams::Streams(const std::shared_ptr<StreamsAdapter> &adapter)
     : key_streams_(std::move(adapter->GetKeyStreams())),
       stream_capabilities_(std::move(adapter->GetStreamCapabilities())),
       unpack_img_data_map_(std::move(adapter->GetUnpackImgDataMap())),
-      unpack_img_pixels_map_(std::move(adapter->GetUnpackImgPixelsMap())) {
+      unpack_img_pixels_map_(std::move(adapter->GetUnpackImgPixelsMap())),
+      timestamp_compensate_(0),
+      is_nearly_before_timestamp_limmit_(0),
+      current_datum_(0) {
   VLOG(2) << __func__;
 }
 
@@ -44,6 +48,23 @@ void Streams::ConfigStream(
   stream_config_requests_[capability] = request;
 }
 
+void Streams::CheckTimeStampLimmit(std::shared_ptr<ImgData> img) {
+    img->timestamp += (timestamp_compensate_ * 42949672960);
+  // the timestamp nearly 8 imu frame before limmit
+  if ((img->timestamp % 42949672960) > 42949606770 &&
+      is_nearly_before_timestamp_limmit_ == 0) {
+    current_datum_ = img->timestamp;
+    timestamp_compensate_++;
+    is_nearly_before_timestamp_limmit_ = LIMMIT_IMG_CHECK_DORMANCY_THRESHOLD;
+  }
+  if (is_nearly_before_timestamp_limmit_ > 0) {
+    is_nearly_before_timestamp_limmit_--;
+    if (abs(current_datum_ - img->timestamp) > (u_int64_t)(42949672960/2)) {  // NOLINT
+      img->timestamp -= 42949672960;
+    }
+  }
+}
+
 bool Streams::PushStream(const Capabilities &capability, const void *data) {
   if (!HasStreamConfigRequest(capability)) {
     LOG(FATAL) << "Cannot push stream without stream config request";
@@ -61,6 +82,7 @@ bool Streams::PushStream(const Capabilities &capability, const void *data) {
       if (unpack_img_data_map_[Stream::LEFT](
               data, request, left_data.img.get())) {
         left_data.frame_id = left_data.img->frame_id;
+        CheckTimeStampLimmit(left_data.img);
         // alloc right
         AllocStreamData(capability, Stream::RIGHT, request);
         auto &&right_data = stream_datas_map_[Stream::RIGHT].back();

src/mynteye/device/streams.h

@@ -27,6 +27,7 @@
 #include "mynteye/device/callbacks.h"
 
 MYNTEYE_BEGIN_NAMESPACE
+#define LIMMIT_IMG_CHECK_DORMANCY_THRESHOLD 10;
 
 class StreamsAdapter;
 
@@ -58,6 +59,7 @@ class Streams {
   stream_data_t GetLatestStreamData(const Stream &stream);
 
   const stream_datas_t &stream_datas(const Stream &stream);
+  void CheckTimeStampLimmit(std::shared_ptr<ImgData> img);
 
  private:
   bool IsStreamCapability(const Capabilities &capability) const;
@@ -90,6 +92,9 @@ class Streams {
 
   std::mutex mtx_;
   std::condition_variable cv_;
+  uint32_t timestamp_compensate_;
+  uint16_t is_nearly_before_timestamp_limmit_;
+  uint64_t current_datum_;
 };
 
 class StreamsAdapter {