fix(timestamp): add timestamp compensate for overflow value.

src/mynteye/device/channel/channels.cc

@@ -142,7 +142,10 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
     imu_track_stop_(false),
     imu_sn_(0),
     imu_callback_(nullptr),
-    dev_info_(nullptr) {
+    dev_info_(nullptr),
+    timestamp_compensate_(0),
+    is_nearly_before_timestamp_limmit_(0),
+    current_datum_(0) {
   VLOG(2) << __func__;
   UpdateControlInfos();
   accel_range = GetControlValue(Option::ACCELEROMETER_RANGE);
@@ -457,14 +460,40 @@ void Channels::DoImuTrack1() {
   imu_sn_ = sn;
 
   if (imu_callback_) {
-    for (auto &&packet : res_packet.packets) {
-      imu_callback_(to_pak2(packet, accel_range, gyro_range));
+    for (auto &packet : res_packet.packets) {
+      auto pak2_tmp = to_pak2(packet, accel_range, gyro_range);
+      checkTimeStampLimmit(pak2_tmp);
+      imu_callback_(pak2_tmp);
     }
   }
-
   res_packet.packets.clear();
 }
 
+void Channels::checkTimeStampLimmit(mynteye::ImuPacket2 &packet) {
+  for (auto &segment2 : packet.segments) {
+    segment2.timestamp += (timestamp_compensate_ * 42949672960);
+    // the timestamp nearly 8 imu frame before limmit
+    if ((segment2.timestamp % 42949672960) > 42949606770 &&
+        is_nearly_before_timestamp_limmit_ == 0) {
+      current_datum_ = segment2.timestamp;
+      timestamp_compensate_++;
+      is_nearly_before_timestamp_limmit_ = LIMMIT_CHECK_DORMANCY_THRESHOLD;
+    }
+    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;
+      }
+    }
+  }
+}
+
 void Channels::DoImuTrack2() {
   // LOG(INFO) << "wait to adapter!";
   static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
@@ -495,7 +524,8 @@ void Channels::DoImuTrack2() {
   }
   imu_sn_ = sn;
   if (imu_callback_) {
-    for (auto &&packet : res_packet.packets) {
+    for (auto &packet : res_packet.packets) {
+      checkTimeStampLimmit(packet);
       imu_callback_(packet);
     }
   }

src/mynteye/device/channel/channels.h

@@ -28,6 +28,8 @@
 #include "mynteye/device/types.h"
 #include "mynteye/uvc/uvc.h"
 
+#define LIMMIT_CHECK_DORMANCY_THRESHOLD 100
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace uvc {
@@ -93,6 +95,9 @@ class MYNTEYE_API Channels {
   inline void EnableImuCorrespondence(bool is_enable) {
     enable_imu_correspondence = is_enable;
   }
+  inline uint64_t timestamp_compensate(uint32_t timestamp32) {
+    return 0;
+  }
 
  private:
   bool PuControlRange(
@@ -126,6 +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);
 
   control_info_t PuControlInfo(Option option) const;
   control_info_t XuControlInfo(Option option) const;
@@ -144,10 +150,12 @@ class MYNTEYE_API Channels {
   volatile bool imu_track_stop_;
   int accel_range;
   int gyro_range;
-
   std::uint32_t imu_sn_;
   imu_callback_t imu_callback_;
   std::shared_ptr<device_info_t> dev_info_;
+  uint32_t timestamp_compensate_;
+  uint16_t is_nearly_before_timestamp_limmit_;
+  uint64_t current_datum_;
 };
 
 class ChannelsAdapter {

src/mynteye/device/channel/def.h

@@ -21,6 +21,9 @@
 
 #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

@@ -49,7 +49,18 @@ struct ImuData {
 void unpack_imu_segment(const ImuData &imu, const std::uint32_t &timestamp,
     ImuSegment *seg) {
   seg->frame_id = static_cast<uint32_t>(imu.frame_id);
+#if DEBUG_TIME_LIMIT
+  std::uint32_t timestamp_test =
+      timestamp + imu.offset + (std::uint32_t)4290000000;
+  seg->timestamp = static_cast<uint64_t>(timestamp_test) * 10;
+  LOG(WARNING) << "timestamp_out: "
+               << seg->timestamp
+               << "  timestamp: "
+               << timestamp << "   timestamp_test: "
+               << timestamp_test;
+#else
   seg->timestamp = static_cast<uint64_t>(timestamp + imu.offset) * 10;
+#endif
   seg->flag = 0;
   seg->temperature = imu.temperature;
   seg->accel[0] = imu.accel[0];