From 817057875b71f8087edf88b511d8d786901b1cd6 Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Fri, 27 Sep 2019 18:15:03 +0800
Subject: [PATCH 1/5] feat(imu): imu 2.0 protocl.

---
 src/mynteye/device/channel/channels.cc        | 161 ++++++++++++------
 src/mynteye/device/channel/channels.h         |  12 +-
 src/mynteye/device/channel/def.h              |  72 ++++++++
 src/mynteye/device/motions.cc                 |   2 +-
 .../device/standard/channels_adapter_s.cc     |   5 +
 .../device/standard/channels_adapter_s.h      |   1 +
 .../device/standard2/channels_adapter_s2.cc   |  94 ++++++++++
 .../device/standard2/channels_adapter_s2.h    |   1 +
 8 files changed, 297 insertions(+), 51 deletions(-)

diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc
index 34af9fc..5c0bd5c 100644
--- a/src/mynteye/device/channel/channels.cc
+++ b/src/mynteye/device/channel/channels.cc
@@ -30,6 +30,30 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
+mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1) {
+  mynteye::ImuPacket2 res;
+  res.version = pak1.version;
+  res.count = pak1.count;
+  res.serial_number = pak1.serial_number;
+  for (size_t i = 0; i < pak1.segments.size(); i++) {
+    mynteye::ImuSegment2 tpr;
+    tpr.accel[0] = pak1.segments[i].accel[0];
+    tpr.accel[1] = pak1.segments[i].accel[1];
+    tpr.accel[2] = pak1.segments[i].accel[2];
+    tpr.gyro[0] = pak1.segments[i].gyro[0];
+    tpr.gyro[1] = pak1.segments[i].gyro[1];
+    tpr.gyro[2] = pak1.segments[i].gyro[2];
+    tpr.flag = pak1.segments[i].flag;
+    tpr.frame_id = pak1.segments[i].frame_id;
+    tpr.is_ets = pak1.segments[i].is_ets;
+    tpr.temperature = pak1.segments[i].temperature;
+    tpr.timestamp = pak1.segments[i].timestamp;
+    res.segments.push_back(tpr);
+  }
+  return res;
+}
+
+
 namespace {
 
 const uvc::xu mynteye_xu = {3, 2,
@@ -328,7 +352,7 @@ bool Channels::SetControlValue(const Option &option, std::uint64_t value) {
     case Option::MIN_EXPOSURE_TIME:
     case Option::IIC_ADDRESS_SETTING:
     case Option::ZERO_DRIFT_CALIBRATION:
-      LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)";
+      LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)";  // NOLINT
       break;
     case Option::ERASE_CHIP:
       LOG(WARNING) << option << " set value useless";
@@ -379,7 +403,7 @@ void Channels::SetImuCallback(imu_callback_t callback) {
 }
 
 void Channels::DoImuTrack() {
-  if (IsImuProc2()) {
+  if (IsImuProtocol2()) {
     return DoImuTrack2();
   } else {
     return DoImuTrack1();
@@ -424,7 +448,7 @@ void Channels::DoImuTrack1() {
 
   if (imu_callback_) {
     for (auto &&packet : res_packet.packets) {
-      imu_callback_(packet);
+      imu_callback_(to_pak2(packet));
     }
   }
 
@@ -432,49 +456,48 @@ void Channels::DoImuTrack1() {
 }
 
 void Channels::DoImuTrack2() {
-  // static ImuReqPacket req_packet{0};
-  // static ImuResPacket res_packet;
+  // LOG(INFO) << "wait to adapter!";
+  static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
+  static ImuResPacket2 res_packet;
+  LOG(INFO) << 1;
+  if (!XuImuWrite(req_packet)) {
+    return;
+  }
+LOG(INFO) << 2;
+  if (!XuImuRead(&res_packet)) {
+    return;
+  }
+LOG(INFO) << 3;
+  if (res_packet.packets.size() == 0) {
+    return;
+  }
+LOG(INFO) << 4;
+  if (res_packet.packets.back().count == 0) {
+    return;
+  }
+LOG(INFO) << 5;
+  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;
+  }();
+LOG(INFO) << 6;
+  auto &&sn = res_packet.packets.back().serial_number;
+  if (imu_sn_ == sn) {
+    VLOG(2) << "New imu not ready, dropped";
+    return;
+  }
+  imu_sn_ = sn;
+LOG(INFO) << 7;
+  if (imu_callback_) {
+    for (auto &&packet : res_packet.packets) {
+      imu_callback_(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) {
-  //     imu_callback_(packet);
-  //   }
-  // }
-
-  // res_packet.packets.clear();
-  LOG(INFO) << "wait to adapter!";
+  res_packet.packets.clear();
 }
 
 
@@ -499,7 +522,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
                 << ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
       }
     };
-    if (IsImuProc2()) {
+    if (IsImuProtocol2()) {
       while (!imu_track_stop_) {
         auto &&time_beg = times::now();
         DoImuTrack2();
@@ -801,7 +824,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
                            static_cast<std::uint8_t>((value >> 56) & 0xFF)};
 
   if (XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 20, data)) {
-    VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value
+    VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value  // NOLINT
             << ") of " << option << " success";
     return true;
   } else {
@@ -813,7 +836,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
 
 bool Channels::XuImuWrite(const ImuReqPacket &req) const {
   auto &&data = req.to_data();
-  // LOG(INFO) << data.size() << "||" << (int)data[0] << " " <<  (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4];
+  // LOG(INFO) << data.size() << "||" << (int)data[0] << " " <<  (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4];  // NOLINT
   if (XuControlQuery(
           CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
     VLOG(2) << "XuImuWrite request success";
@@ -824,6 +847,48 @@ bool Channels::XuImuWrite(const ImuReqPacket &req) const {
   }
 }
 
+bool Channels::XuImuWrite(const ImuReqPacket2 &req) const {
+  auto &&data = req.to_data();
+  // LOG(INFO) << data.size() << "||" << (int)data[0] << " " <<  (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4];  // NOLINT
+  if (XuControlQuery(
+          CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
+    VLOG(2) << "XuImuWrite request success";
+    return true;
+  } else {
+    LOG(WARNING) << "XuImuWrite request failed";
+    return false;
+  }
+}
+
+bool Channels::XuImuRead(ImuResPacket2 *res) const {
+  static std::uint8_t data[2000]{};
+  if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
+    adapter_->GetImuResPacket2(data, res);
+    if (res->header != 0x5B) {
+      LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
+                  << std::hex << std::uppercase << std::setw(2)
+                  << std::setfill('0') << static_cast<int>(res->header)
+                  << " now";
+      return false;
+    }
+    if (res->state != 0) {
+      LOG(WARNING) << "Imu response packet state must be 0, but " << res->state  // NOLINT
+                  << " now";
+      return false;
+    }
+    // LOG(INFO) << res->size;
+    if (enable_imu_correspondence) {
+      LOG(INFO) << "enable_imu_correspondence";
+    } else {
+    }
+    VLOG(2) << "XuImuRead response success";
+    return true;
+  } else {
+    LOG(WARNING) << "XuImuRead response failed";
+    return false;
+  }
+}
+
 bool Channels::XuImuRead(ImuResPacket *res) const {
   static std::uint8_t data[2000]{};
   // std::fill(data, data + 2000, 0);  // reset
diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h
index 20c6e38..7e623fd 100644
--- a/src/mynteye/device/channel/channels.h
+++ b/src/mynteye/device/channel/channels.h
@@ -54,7 +54,7 @@ class MYNTEYE_API Channels {
     XU_CMD_LAST
   } xu_cmd_t;
 
-  using imu_callback_t = std::function<void(const ImuPacket &packet)>;
+  using imu_callback_t = std::function<void(const ImuPacket2 &packet)>;
 
   using device_info_t = FileChannel::device_info_t;
   using img_params_t = FileChannel::img_params_t;
@@ -89,7 +89,10 @@ class MYNTEYE_API Channels {
       device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
   bool SetFiles(
       device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
-  inline bool IsImuProc2() const { return is_imu_proto2_; }
+  inline bool IsImuProtocol2() const { return is_imu_proto2_; }
+  inline void EnableImuCorrespondence(bool is_enable) {
+    enable_imu_correspondence = is_enable;
+  }
 
  private:
   bool PuControlRange(
@@ -118,7 +121,9 @@ class MYNTEYE_API Channels {
   bool XuHalfDuplexSet(Option option, std::uint64_t value) const;
 
   bool XuImuWrite(const ImuReqPacket &req) const;
+  bool XuImuWrite(const ImuReqPacket2 &req) const;
   bool XuImuRead(ImuResPacket *res) const;
+  bool XuImuRead(ImuResPacket2 *res) const;
 
   bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
 
@@ -134,6 +139,7 @@ class MYNTEYE_API Channels {
 
   bool is_imu_tracking_;
   bool is_imu_proto2_;
+  bool enable_imu_correspondence;
   std::thread imu_track_thread_;
   volatile bool imu_track_stop_;
 
@@ -157,6 +163,8 @@ class ChannelsAdapter {
   virtual std::vector<std::int32_t> GetGyroRangeValues() = 0;
 
   virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
+  virtual void GetImuResPacket2(const std::uint8_t *data,
+      ImuResPacket2 *res) = 0;
 
  protected:
   Model model_;
diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h
index ec8b2b9..ee10bd9 100644
--- a/src/mynteye/device/channel/def.h
+++ b/src/mynteye/device/channel/def.h
@@ -63,6 +63,34 @@ struct ImuReqPacket {
 };
 #pragma pack(pop)
 
+/**
+ * @ingroup datatypes
+ * Imu request packet 2.0.
+ */
+#pragma pack(push, 1)
+struct ImuReqPacket2 {
+  std::uint8_t header;
+  std::uint32_t serial_number;
+  std::uint8_t correspondence_switch;
+
+  ImuReqPacket2() = default;
+  explicit ImuReqPacket2(std::uint32_t serial_number)
+      : ImuReqPacket2(0x5A, serial_number) {}
+  ImuReqPacket2(std::uint8_t header, std::uint32_t serial_number)
+      : ImuReqPacket2(header, serial_number, false) {}
+  ImuReqPacket2(std::uint8_t header,
+      std::uint32_t serial_number,
+      bool correspondence_switch_in)
+      : header(header),
+        serial_number(serial_number),
+        correspondence_switch(correspondence_switch_in ? 1:0) {}
+
+  std::array<std::uint8_t, 5> to_data() const {
+    return {header, correspondence_switch, 0, 0, 0};
+  }
+};
+#pragma pack(pop)
+
 /**
  * @ingroup datatypes
  * Imu segment.
@@ -80,6 +108,23 @@ struct ImuSegment {
 };
 #pragma pack(pop)
 
+/**
+ * @ingroup datatypes
+ * Imu segment.
+ */
+#pragma pack(push, 1)
+struct ImuSegment2 {
+  std::uint32_t frame_id;
+  std::uint64_t timestamp;
+  std::uint8_t flag;
+  // Is external time source
+  bool is_ets;
+  std::int32_t temperature;
+  std::int32_t accel[3];
+  std::int32_t gyro[3];
+};
+#pragma pack(pop)
+
 /**
  * @ingroup datatypes
  * Imu packet.
@@ -92,6 +137,18 @@ struct ImuPacket {
   std::vector<ImuSegment> segments;
 };
 #pragma pack(pop)
+/**
+ * @ingroup datatypes
+ * Imu packet.
+ */
+#pragma pack(push, 1)
+struct ImuPacket2 {
+  std::uint8_t version;
+  std::uint8_t count;
+  std::uint32_t serial_number;
+  std::vector<ImuSegment2> segments;
+};
+#pragma pack(pop)
 
 /**
  * @ingroup datatypes
@@ -108,6 +165,21 @@ struct ImuResPacket {
 };
 #pragma pack(pop)
 
+/**
+ * @ingroup datatypes
+ * Imu response packet.
+ */
+#pragma pack(push, 1)
+struct ImuResPacket2 {
+  std::uint8_t version;
+  std::uint8_t header;
+  std::uint8_t state;
+  std::uint16_t size;
+  std::vector<ImuPacket2> packets;
+  std::uint8_t checksum;
+};
+#pragma pack(pop)
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_DEVICE_CHANNEL_DEF_H_
diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc
index 38febaa..e48d6fc 100644
--- a/src/mynteye/device/motions.cc
+++ b/src/mynteye/device/motions.cc
@@ -70,7 +70,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
     if (gyro_range == -1)
       gyro_range = channels_->GetGyroRangeDefault();
 
-    channels_->SetImuCallback([this](const ImuPacket &packet) {
+    channels_->SetImuCallback([this](const ImuPacket2 &packet) {
       if (!motion_callback_ && !motion_datas_enabled_) {
         return;
       }
diff --git a/src/mynteye/device/standard/channels_adapter_s.cc b/src/mynteye/device/standard/channels_adapter_s.cc
index 118ac29..b45fa2e 100644
--- a/src/mynteye/device/standard/channels_adapter_s.cc
+++ b/src/mynteye/device/standard/channels_adapter_s.cc
@@ -122,4 +122,9 @@ void StandardChannelsAdapter::GetImuResPacket(
   unpack_imu_res_packet(data, res);
 }
 
+void StandardChannelsAdapter::GetImuResPacket2(
+    const std::uint8_t *data, ImuResPacket2 *res) {
+  LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
+}
+
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/standard/channels_adapter_s.h b/src/mynteye/device/standard/channels_adapter_s.h
index bc3fcbd..c0c9fd1 100644
--- a/src/mynteye/device/standard/channels_adapter_s.h
+++ b/src/mynteye/device/standard/channels_adapter_s.h
@@ -35,6 +35,7 @@ class StandardChannelsAdapter : public ChannelsAdapter {
   std::vector<std::int32_t> GetGyroRangeValues() override;
 
   void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
+  void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override;
 };
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 948e17e..edf1829 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -52,6 +52,50 @@ struct ImuData {
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
+
+#define BYTE_8(data, begin) (static_cast<std::uint64_t>( \
+                ((*(data + begin) << 24) | (*(data + begin+1) << 16) | \
+                  (*(data + begin+2) << 8) | *(data + begin+3)) << 32) | \
+                  ((*(data + begin+4) << 24) | (*(data + begin+5) << 16) | \
+                  (*(data + begin+6) << 8) | *(data + begin+7)))
+#define BYTE_4(data, begin) (*(data + begin) << 24) | \
+                    (*(data + begin + 1) << 16) | \
+                    (*(data + begin + 2) << 8) | \
+                    *(data + begin + 3)
+struct ImuData2 {
+  std::uint32_t frame_id;
+  std::uint64_t timestamp;
+  std::uint8_t flag;
+  float temperature;
+  float accel_or_gyro[3];
+
+  ImuData2() = default;
+  explicit ImuData2(const std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(const std::uint8_t *data) {
+    std::uint32_t timestamp_l;
+    std::uint32_t timestamp_h;
+
+    frame_id = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
+                    *(data + 3);
+    // timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
+    //               (*(data + 6) << 8) | *(data + 7);
+    // timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
+    //               (*(data + 10) << 8) | *(data + 11);
+    timestamp = BYTE_8(data, 4);
+    flag = *(data + 12);
+    temperature = BYTE_4(data, 13);
+    // accel_or_gyro[0] = BYTE_4(data, 17);
+    // accel_or_gyro[1] = BYTE_4(data, 21);
+    // accel_or_gyro[2] = BYTE_4(data, 25);
+  }
+};
+
+#pragma pack(pop)
+
 void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
   seg->frame_id = imu.frame_id;
   seg->timestamp = imu.timestamp;
@@ -66,6 +110,21 @@ void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
   seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
 }
 
+void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
+  LOG(INFO) << "unpack_imu_segment2" << imu.timestamp;
+  seg->frame_id = imu.frame_id;
+  seg->timestamp = imu.timestamp;
+  seg->flag = imu.flag & 0b0011;
+  seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
+  seg->temperature = imu.temperature;
+  seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
+  seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;
+  seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0;
+  seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0;
+  seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0;
+  seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
+}
+
 void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
   std::size_t data_n = sizeof(ImuData);  // 21
   for (std::size_t i = 0; i < pkg->count; i++) {
@@ -81,6 +140,21 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
   }
 }
 
+void unpack_imu_packet2(const std::uint8_t *data, ImuPacket2 *pkg) {
+  std::size_t data_n = sizeof(ImuData2);  // 29
+  for (std::size_t i = 0; i < pkg->count; i++) {
+    ImuSegment2 seg;
+    unpack_imu_segment2(ImuData2(data + data_n * i), &seg);
+    pkg->segments.push_back(seg);
+  }
+  if (pkg->count) {
+    pkg->serial_number = pkg->segments.back().frame_id;
+  } else {
+    LOG(ERROR) << "The imu data pipeline lost more than 5 samples continuously, "
+               << "please check the device and firmware";
+  }
+}
+
 void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
   res->header = *data;
   res->state = *(data + 1);
@@ -94,6 +168,21 @@ void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
   res->checksum = *(data + 4 + res->size);
 }
 
+void unpack_imu_res_packet2(const std::uint8_t *data, ImuResPacket2 *res) {
+  res->header = *data;
+  // u_int64_t* jj = (u_int64_t*) data;
+  res->state = *(data + 1);
+  res->size = (*(data + 2) << 8) | *(data + 3);
+  std::size_t data_n = sizeof(ImuData2);  // 21
+  // LOG(INFO) << "size:" << data_n;
+  ImuPacket2 packet;
+  packet.count = res->size / data_n;
+  LOG(INFO) << "count:" << (int)(packet.count);
+  unpack_imu_packet2(data + 4, &packet);
+  res->packets.push_back(packet);
+  res->checksum = *(data + 4 + res->size);
+}
+
 }  // namespace
 
 Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
@@ -124,4 +213,9 @@ void Standard2ChannelsAdapter::GetImuResPacket(
   unpack_imu_res_packet(data, res);
 }
 
+void Standard2ChannelsAdapter::GetImuResPacket2(
+    const std::uint8_t *data, ImuResPacket2 *res) {
+  unpack_imu_res_packet2(data, res);
+}
+
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.h b/src/mynteye/device/standard2/channels_adapter_s2.h
index 7301be0..64510b1 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.h
+++ b/src/mynteye/device/standard2/channels_adapter_s2.h
@@ -35,6 +35,7 @@ class Standard2ChannelsAdapter : public ChannelsAdapter {
   std::vector<std::int32_t> GetGyroRangeValues() override;
 
   void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
+  void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override;
 };
 
 MYNTEYE_END_NAMESPACE

From 997b6102d4b4c8f1624a0d9bf79ad3d10d554f4b Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Fri, 27 Sep 2019 18:27:55 +0800
Subject: [PATCH 2/5] feat(imu): adapter data right.

---
 src/mynteye/device/standard2/channels_adapter_s2.cc | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index edf1829..8083b88 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -87,10 +87,14 @@ struct ImuData2 {
     //               (*(data + 10) << 8) | *(data + 11);
     timestamp = BYTE_8(data, 4);
     flag = *(data + 12);
-    temperature = BYTE_4(data, 13);
-    // accel_or_gyro[0] = BYTE_4(data, 17);
-    // accel_or_gyro[1] = BYTE_4(data, 21);
-    // accel_or_gyro[2] = BYTE_4(data, 25);
+    temperature = *((float*)(data+ 13));
+    LOG(INFO) << "temperature:" << temperature;
+    accel_or_gyro[0] = *((float*)(data + 17));
+    LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0];
+    accel_or_gyro[1] = *((float*)(data + 21));
+    LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
+    accel_or_gyro[2] = *((float*)(data + 25));
+    LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2];
   }
 };
 

From 7e0e4b83e72fc6205a6d89a1d571d85b8be5f48e Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Sun, 29 Sep 2019 15:11:16 +0800
Subject: [PATCH 3/5] feat(imu): imu2.0 first complete.

---
 samples/util_cv.cc                            | 19 +-----
 src/mynteye/device/channel/channels.cc        | 45 ++++++++------
 src/mynteye/device/channel/channels.h         |  2 +
 src/mynteye/device/channel/def.h              |  6 +-
 src/mynteye/device/motions.cc                 | 35 ++++++++---
 .../device/standard2/channels_adapter_s2.cc   | 61 +++++++++++--------
 6 files changed, 97 insertions(+), 71 deletions(-)

diff --git a/samples/util_cv.cc b/samples/util_cv.cc
index 1911d78..6e4b547 100644
--- a/samples/util_cv.cc
+++ b/samples/util_cv.cc
@@ -141,23 +141,8 @@ cv::Rect CVPainter::DrawImuData(
   cv::Rect rect_a =
       DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
 
-  static double gyro1_s = 0.0;
-  static double gyro2_s = 0.0;
-  static double gyro3_s = 0.0;
-
-  if (data.gyro[0] > 0.01 ||
-      data.gyro[1] > 0.01 ||
-      data.gyro[2] > 0.01 ||
-      data.gyro[0] < -0.01 ||
-      data.gyro[1] < -0.01 ||
-      data.gyro[2] < -0.01 ) {
-    gyro1_s = data.gyro[0];
-    gyro2_s = data.gyro[1];
-    gyro3_s = data.gyro[2];
-  }
-
-  Clear(ss) << "gyro(x,y,z): " << fmt_imu << gyro1_s << "," << fmt_imu
-            << gyro2_s << "," << fmt_imu << gyro3_s;
+  Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
+            << data.gyro[1] << "," << fmt_imu << data.gyro[2];
   cv::Rect rect_g = DrawText(
       img, ss.str(), gravity, 5, 0,
       sign * (10 + rect_i.height + rect_a.height));
diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc
index 5c0bd5c..e838e6c 100644
--- a/src/mynteye/device/channel/channels.cc
+++ b/src/mynteye/device/channel/channels.cc
@@ -30,23 +30,25 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
-mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1) {
+mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1,
+    const int &accel_range,
+    const int &gyro_range) {
   mynteye::ImuPacket2 res;
   res.version = pak1.version;
   res.count = pak1.count;
   res.serial_number = pak1.serial_number;
   for (size_t i = 0; i < pak1.segments.size(); i++) {
     mynteye::ImuSegment2 tpr;
-    tpr.accel[0] = pak1.segments[i].accel[0];
-    tpr.accel[1] = pak1.segments[i].accel[1];
-    tpr.accel[2] = pak1.segments[i].accel[2];
-    tpr.gyro[0] = pak1.segments[i].gyro[0];
-    tpr.gyro[1] = pak1.segments[i].gyro[1];
-    tpr.gyro[2] = pak1.segments[i].gyro[2];
+    tpr.accel[0] = pak1.segments[i].accel[0] * 1.f * accel_range / 0x10000;
+    tpr.accel[1] = pak1.segments[i].accel[1] * 1.f * accel_range / 0x10000;
+    tpr.accel[2] = pak1.segments[i].accel[2] * 1.f * accel_range / 0x10000;
+    tpr.gyro[0] = pak1.segments[i].gyro[0] * gyro_range / 0x10000;
+    tpr.gyro[1] = pak1.segments[i].gyro[1] * gyro_range / 0x10000;
+    tpr.gyro[2] = pak1.segments[i].gyro[2] * gyro_range / 0x10000;
     tpr.flag = pak1.segments[i].flag;
     tpr.frame_id = pak1.segments[i].frame_id;
     tpr.is_ets = pak1.segments[i].is_ets;
-    tpr.temperature = pak1.segments[i].temperature;
+    tpr.temperature = pak1.segments[i].temperature / 326.8f + 25;
     tpr.timestamp = pak1.segments[i].timestamp;
     res.segments.push_back(tpr);
   }
@@ -142,6 +144,13 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
     dev_info_(nullptr) {
   VLOG(2) << __func__;
   UpdateControlInfos();
+  accel_range = GetControlValue(Option::ACCELEROMETER_RANGE);
+  if (accel_range == -1)
+    accel_range = GetAccelRangeDefault();
+
+  gyro_range = GetControlValue(Option::GYROSCOPE_RANGE);
+  if (gyro_range == -1)
+    gyro_range = GetGyroRangeDefault();
 }
 
 Channels::~Channels() {
@@ -448,7 +457,7 @@ void Channels::DoImuTrack1() {
 
   if (imu_callback_) {
     for (auto &&packet : res_packet.packets) {
-      imu_callback_(to_pak2(packet));
+      imu_callback_(to_pak2(packet, accel_range, gyro_range));
     }
   }
 
@@ -459,23 +468,18 @@ void Channels::DoImuTrack2() {
   // LOG(INFO) << "wait to adapter!";
   static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
   static ImuResPacket2 res_packet;
-  LOG(INFO) << 1;
   if (!XuImuWrite(req_packet)) {
     return;
   }
-LOG(INFO) << 2;
   if (!XuImuRead(&res_packet)) {
     return;
   }
-LOG(INFO) << 3;
   if (res_packet.packets.size() == 0) {
     return;
   }
-LOG(INFO) << 4;
   if (res_packet.packets.back().count == 0) {
     return;
   }
-LOG(INFO) << 5;
   VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
     std::size_t n = 0;
     for (auto &&packet : res_packet.packets) {
@@ -483,16 +487,23 @@ LOG(INFO) << 5;
     }
     return n;
   }();
-LOG(INFO) << 6;
   auto &&sn = res_packet.packets.back().serial_number;
   if (imu_sn_ == sn) {
     VLOG(2) << "New imu not ready, dropped";
     return;
   }
   imu_sn_ = sn;
-LOG(INFO) << 7;
   if (imu_callback_) {
     for (auto &&packet : res_packet.packets) {
+      // LOG(INFO) << "packet count:" << (int)packet.count;
+      // for (size_t i = 0; i < (int)packet.count; i++) {  // NOLINT
+      //   LOG(INFO) << "accel:" << packet.segments[i].accel[0];
+      //   LOG(INFO) << packet.segments[i].accel[1];
+      //   LOG(INFO) << packet.segments[i].accel[2];
+      //   LOG(INFO) << "gyro:" << packet.segments[i].gyro[0];
+      //   LOG(INFO) << packet.segments[i].gyro[1];
+      //   LOG(INFO) << packet.segments[i].gyro[2];
+      // }
       imu_callback_(packet);
     }
   }
@@ -888,13 +899,11 @@ bool Channels::XuImuRead(ImuResPacket2 *res) const {
     return false;
   }
 }
-
 bool Channels::XuImuRead(ImuResPacket *res) const {
   static std::uint8_t data[2000]{};
   // std::fill(data, data + 2000, 0);  // reset
   if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
     adapter_->GetImuResPacket(data, res);
-
     if (res->header != 0x5B) {
       LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
                    << std::hex << std::uppercase << std::setw(2)
diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h
index 7e623fd..b8319ce 100644
--- a/src/mynteye/device/channel/channels.h
+++ b/src/mynteye/device/channel/channels.h
@@ -142,6 +142,8 @@ class MYNTEYE_API Channels {
   bool enable_imu_correspondence;
   std::thread imu_track_thread_;
   volatile bool imu_track_stop_;
+  int accel_range;
+  int gyro_range;
 
   std::uint32_t imu_sn_;
   imu_callback_t imu_callback_;
diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h
index ee10bd9..4138ab0 100644
--- a/src/mynteye/device/channel/def.h
+++ b/src/mynteye/device/channel/def.h
@@ -119,9 +119,9 @@ struct ImuSegment2 {
   std::uint8_t flag;
   // Is external time source
   bool is_ets;
-  std::int32_t temperature;
-  std::int32_t accel[3];
-  std::int32_t gyro[3];
+  float temperature;
+  float accel[3];
+  float gyro[3];
 };
 #pragma pack(pop)
 
diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc
index e48d6fc..e901694 100644
--- a/src/mynteye/device/motions.cc
+++ b/src/mynteye/device/motions.cc
@@ -85,13 +85,34 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
         imu->timestamp = seg.timestamp;
         imu->flag = seg.flag;
         imu->is_ets = seg.is_ets;
-        imu->temperature = seg.temperature / 326.8f + 25;
-        imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
-        imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
-        imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
-        imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
-        imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
-        imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
+        // imu->temperature = seg.temperature / 326.8f + 25;
+        imu->temperature = seg.temperature;
+        // LOG(INFO) << "beforea" << seg.accel[0];
+        // LOG(INFO) << "before" << seg.accel[1];
+        // LOG(INFO) << "before" << seg.accel[2];
+        // LOG(INFO) << "beforeg" << seg.gyro[0];
+        // LOG(INFO) << "before" << seg.gyro[1];
+        // LOG(INFO) << "before" << seg.gyro[2];
+        // imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
+        // imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
+        // imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
+        // imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
+        // imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
+        // imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
+
+        imu->accel[0] = seg.accel[0];
+        imu->accel[1] = seg.accel[1];
+        imu->accel[2] = seg.accel[2];
+        imu->gyro[0] = seg.gyro[0];
+        imu->gyro[1] = seg.gyro[1];
+        imu->gyro[2] = seg.gyro[2];
+
+        // LOG(INFO)<< "aftera" << imu->accel[0];
+        // LOG(INFO)<< "after" << imu->accel[1];
+        // LOG(INFO)<< "after" << imu->accel[2];
+        // LOG(INFO)<< "afterg" << imu->gyro[0];
+        // LOG(INFO)<< "afterg" << imu->gyro[1];
+        // LOG(INFO)<< "afterg" << imu->gyro[2];
 
         bool proc_assembly = ((proc_mode_ & ProcessMode::PROC_IMU_ASSEMBLY) > 0);
         bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0);
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 8083b88..39da2f3 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -76,25 +76,17 @@ struct ImuData2 {
   }
 
   void from_data(const std::uint8_t *data) {
-    std::uint32_t timestamp_l;
-    std::uint32_t timestamp_h;
-
-    frame_id = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
-                    *(data + 3);
-    // timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
-    //               (*(data + 6) << 8) | *(data + 7);
-    // timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
-    //               (*(data + 10) << 8) | *(data + 11);
-    timestamp = BYTE_8(data, 4);
+    frame_id = BYTE_4(data, 0);
+    timestamp = BYTE_8((u_char*)data, 4);  // NOLINT
     flag = *(data + 12);
-    temperature = *((float*)(data+ 13));
-    LOG(INFO) << "temperature:" << temperature;
-    accel_or_gyro[0] = *((float*)(data + 17));
-    LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0];
-    accel_or_gyro[1] = *((float*)(data + 21));
-    LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
-    accel_or_gyro[2] = *((float*)(data + 25));
-    LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2];
+    temperature = *((float*)(data+ 13));  // NOLINT
+    // LOG(INFO) << "temperature:" << temperature;
+    accel_or_gyro[0] = *((float*)(data + 17));  // NOLINT
+    // LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0];
+    accel_or_gyro[1] = *((float*)(data + 21));  // NOLINT
+    // LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
+    accel_or_gyro[2] = *((float*)(data + 25));  // NOLINT
+    // LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2];
   }
 };
 
@@ -115,18 +107,36 @@ void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
 }
 
 void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
-  LOG(INFO) << "unpack_imu_segment2" << imu.timestamp;
   seg->frame_id = imu.frame_id;
   seg->timestamp = imu.timestamp;
   seg->flag = imu.flag & 0b0011;
   seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
   seg->temperature = imu.temperature;
-  seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
-  seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;
-  seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0;
-  seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0;
-  seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0;
-  seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
+  if (seg->flag == 1) {
+    // LOG(INFO) << "flag1";
+    seg->accel[0] = imu.accel_or_gyro[0];
+    seg->accel[1] = imu.accel_or_gyro[1];
+    seg->accel[2] = imu.accel_or_gyro[2];
+    seg->gyro[0] = 0.;
+    seg->gyro[1] = 0.;
+    seg->gyro[2] = 0.;
+  } else if (seg->flag == 2) {
+    // LOG(INFO) << "flag2";
+    seg->gyro[0] = imu.accel_or_gyro[0];
+    seg->gyro[1] = imu.accel_or_gyro[1];
+    seg->gyro[2] = imu.accel_or_gyro[2];
+    seg->accel[0] = 0.;
+    seg->accel[1] = 0.;
+    seg->accel[2] = 0.;
+  } else if (seg->flag == 3) {
+    LOG(INFO) << "flag3";
+    // seg->gyro[0] = imu.accel_or_gyro[0];
+    // seg->gyro[1] = imu.accel_or_gyro[1];
+    // seg->gyro[2] = imu.accel_or_gyro[2];
+    // seg->accel[0] = 0.;
+    // seg->accel[1] = 0.;
+    // seg->accel[2] = 0.;
+  }
 }
 
 void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
@@ -181,7 +191,6 @@ void unpack_imu_res_packet2(const std::uint8_t *data, ImuResPacket2 *res) {
   // LOG(INFO) << "size:" << data_n;
   ImuPacket2 packet;
   packet.count = res->size / data_n;
-  LOG(INFO) << "count:" << (int)(packet.count);
   unpack_imu_packet2(data + 4, &packet);
   res->packets.push_back(packet);
   res->checksum = *(data + 4 + res->size);

From 283f0b141c37cca427acd5ee7202c914c4c2f421 Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Sun, 29 Sep 2019 16:42:44 +0800
Subject: [PATCH 4/5] feat(imu): imu2.0 decond part complete.

---
 include/mynteye/api/api.h                     | 10 +++-
 include/mynteye/device/device.h               |  4 ++
 samples/get_imu.cc                            |  2 +
 src/mynteye/api/api.cc                        | 26 ++++++++-
 src/mynteye/device/channel/channels.cc        | 12 +---
 src/mynteye/device/channel/channels.h         |  2 +-
 src/mynteye/device/device.cc                  |  4 ++
 .../device/standard/channels_adapter_s.cc     |  2 +-
 .../device/standard/channels_adapter_s.h      |  5 +-
 .../device/standard2/channels_adapter_s2.cc   | 55 ++++++++++++-------
 .../device/standard2/channels_adapter_s2.h    |  5 +-
 11 files changed, 90 insertions(+), 37 deletions(-)

diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h
index 1fc69bd..a14d7e0 100644
--- a/include/mynteye/api/api.h
+++ b/include/mynteye/api/api.h
@@ -347,7 +347,12 @@ class MYNTEYE_API API {
   std::vector<api::MotionData> GetMotionDatas();
 
   /**
-   * Enable motion datas with timestamp correspondence of some stream.
+   * enable motion datas timestamp correspondence in device.
+   */
+  void EnableImuTimestampCorrespondence(bool is_enable);
+
+  /**
+   * Enable motion datas with timestamp correspondence of some stream in api.
    */
   void EnableTimestampCorrespondence(const Stream &stream,
         bool keep_accel_then_gyro = true);
@@ -385,6 +390,9 @@ class MYNTEYE_API API {
 
   motion_callback_t callback_;
 
+  bool api_correspondence_enable_;
+  bool dev_correspondence_enable_;
+
   void CheckImageParams();
 };
 
diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h
index 05c94e9..3f2ab16 100644
--- a/include/mynteye/device/device.h
+++ b/include/mynteye/device/device.h
@@ -290,6 +290,10 @@ class MYNTEYE_API Device {
    * Enable cache motion datas.
    */
   void EnableMotionDatas();
+  /**
+   * Enable motion datas timestamp correspondence.
+   */
+  void EnableImuCorrespondence(bool is_enable);
   /**
    * Enable cache motion datas.
    */
diff --git a/samples/get_imu.cc b/samples/get_imu.cc
index a0341c3..8f6be00 100644
--- a/samples/get_imu.cc
+++ b/samples/get_imu.cc
@@ -31,6 +31,8 @@ int main(int argc, char *argv[]) {
 
   // Enable this will cache the motion datas until you get them.
   api->EnableMotionDatas();
+  // Enable imu timestamp correspondence int device;
+  api->EnableImuTimestampCorrespondence(true);
 
   api->Start(Source::ALL);
 
diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc
index 6d58ea8..e7e37b8 100644
--- a/src/mynteye/api/api.cc
+++ b/src/mynteye/api/api.cc
@@ -213,7 +213,9 @@ std::vector<std::string> get_plugin_paths() {
 }  // namespace
 
 API::API(std::shared_ptr<Device> device, CalibrationModel calib_model)
-    : device_(device), correspondence_(nullptr) {
+    : device_(device), correspondence_(nullptr),
+      api_correspondence_enable_(false),
+      dev_correspondence_enable_(false) {
   VLOG(2) << __func__;
   // std::dynamic_pointer_cast<StandardDevice>(device_);
   synthetic_.reset(new Synthetic(this, calib_model));
@@ -505,6 +507,15 @@ std::vector<api::MotionData> API::GetMotionDatas() {
 
 void API::EnableTimestampCorrespondence(const Stream &stream,
     bool keep_accel_then_gyro) {
+  if (!dev_correspondence_enable_) {
+    api_correspondence_enable_ = keep_accel_then_gyro;
+  } else {
+    LOG(WARNING) << "dev_correspondence_enable_ "
+                    "has been set to true, "
+                    "you should close it first when you want to use "
+                    "api_correspondence_enable_.";
+    return;
+  }
   if (correspondence_ == nullptr) {
     correspondence_.reset(new Correspondence(device_, stream));
     correspondence_->KeepAccelThenGyro(keep_accel_then_gyro);
@@ -526,6 +537,19 @@ void API::EnableTimestampCorrespondence(const Stream &stream,
   }
 }
 
+void API::EnableImuTimestampCorrespondence(bool is_enable) {
+  if (!api_correspondence_enable_) {
+     dev_correspondence_enable_= is_enable;
+  } else {
+    LOG(WARNING) << "api_correspondence_enable_ "
+                    "has been set to true, "
+                    "you should close it first when you want to use "
+                    "dev_correspondence_enable_.";
+    return;
+  }
+  device_->EnableImuCorrespondence(is_enable);
+}
+
 void API::EnablePlugin(const std::string &path) {
   static DL dl;
   CHECK(dl.Open(path.c_str())) << "Open plugin failed: " << path;
diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc
index e838e6c..4134e03 100644
--- a/src/mynteye/device/channel/channels.cc
+++ b/src/mynteye/device/channel/channels.cc
@@ -138,6 +138,7 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
     adapter_(adapter),
     is_imu_tracking_(false),
     is_imu_proto2_(false),
+    enable_imu_correspondence(false),
     imu_track_stop_(false),
     imu_sn_(0),
     imu_callback_(nullptr),
@@ -495,15 +496,6 @@ void Channels::DoImuTrack2() {
   imu_sn_ = sn;
   if (imu_callback_) {
     for (auto &&packet : res_packet.packets) {
-      // LOG(INFO) << "packet count:" << (int)packet.count;
-      // for (size_t i = 0; i < (int)packet.count; i++) {  // NOLINT
-      //   LOG(INFO) << "accel:" << packet.segments[i].accel[0];
-      //   LOG(INFO) << packet.segments[i].accel[1];
-      //   LOG(INFO) << packet.segments[i].accel[2];
-      //   LOG(INFO) << "gyro:" << packet.segments[i].gyro[0];
-      //   LOG(INFO) << packet.segments[i].gyro[1];
-      //   LOG(INFO) << packet.segments[i].gyro[2];
-      // }
       imu_callback_(packet);
     }
   }
@@ -874,7 +866,7 @@ bool Channels::XuImuWrite(const ImuReqPacket2 &req) const {
 bool Channels::XuImuRead(ImuResPacket2 *res) const {
   static std::uint8_t data[2000]{};
   if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
-    adapter_->GetImuResPacket2(data, res);
+    adapter_->GetImuResPacket2(data, res, enable_imu_correspondence);
     if (res->header != 0x5B) {
       LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
                   << std::hex << std::uppercase << std::setw(2)
diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h
index b8319ce..5901d1b 100644
--- a/src/mynteye/device/channel/channels.h
+++ b/src/mynteye/device/channel/channels.h
@@ -166,7 +166,7 @@ class ChannelsAdapter {
 
   virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
   virtual void GetImuResPacket2(const std::uint8_t *data,
-      ImuResPacket2 *res) = 0;
+      ImuResPacket2 *res, bool is_correspondence_on) = 0;
 
  protected:
   Model model_;
diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc
index 0c4de95..6fb07b8 100644
--- a/src/mynteye/device/device.cc
+++ b/src/mynteye/device/device.cc
@@ -602,6 +602,10 @@ void Device::StopMotionTracking() {
 
 void Device::OnStereoStreamUpdate() {}
 
+void Device::EnableImuCorrespondence(bool is_enable) {
+  channels_->EnableImuCorrespondence(is_enable);
+}
+
 void Device::ReadAllInfos() {
   device_info_ = std::make_shared<DeviceInfo>();
 
diff --git a/src/mynteye/device/standard/channels_adapter_s.cc b/src/mynteye/device/standard/channels_adapter_s.cc
index b45fa2e..237a0c4 100644
--- a/src/mynteye/device/standard/channels_adapter_s.cc
+++ b/src/mynteye/device/standard/channels_adapter_s.cc
@@ -123,7 +123,7 @@ void StandardChannelsAdapter::GetImuResPacket(
 }
 
 void StandardChannelsAdapter::GetImuResPacket2(
-    const std::uint8_t *data, ImuResPacket2 *res) {
+    const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
   LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
 }
 
diff --git a/src/mynteye/device/standard/channels_adapter_s.h b/src/mynteye/device/standard/channels_adapter_s.h
index c0c9fd1..8307d1e 100644
--- a/src/mynteye/device/standard/channels_adapter_s.h
+++ b/src/mynteye/device/standard/channels_adapter_s.h
@@ -35,7 +35,10 @@ class StandardChannelsAdapter : public ChannelsAdapter {
   std::vector<std::int32_t> GetGyroRangeValues() override;
 
   void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
-  void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override;
+  void GetImuResPacket2(
+      const std::uint8_t *data,
+      ImuResPacket2 *res,
+      bool is_correspondence_on) override;
 };
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 39da2f3..4e6ba06 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -54,11 +54,6 @@ struct ImuData {
 
 #pragma pack(push, 1)
 
-#define BYTE_8(data, begin) (static_cast<std::uint64_t>( \
-                ((*(data + begin) << 24) | (*(data + begin+1) << 16) | \
-                  (*(data + begin+2) << 8) | *(data + begin+3)) << 32) | \
-                  ((*(data + begin+4) << 24) | (*(data + begin+5) << 16) | \
-                  (*(data + begin+6) << 8) | *(data + begin+7)))
 #define BYTE_4(data, begin) (*(data + begin) << 24) | \
                     (*(data + begin + 1) << 16) | \
                     (*(data + begin + 2) << 8) | \
@@ -69,6 +64,7 @@ struct ImuData2 {
   std::uint8_t flag;
   float temperature;
   float accel_or_gyro[3];
+  float gyro_add[3];
 
   ImuData2() = default;
   explicit ImuData2(const std::uint8_t *data) {
@@ -76,8 +72,14 @@ struct ImuData2 {
   }
 
   void from_data(const std::uint8_t *data) {
+    std::uint32_t timestamp_l;
+    std::uint32_t timestamp_h;
     frame_id = BYTE_4(data, 0);
-    timestamp = BYTE_8((u_char*)data, 4);  // NOLINT
+    timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
+                  (*(data + 6) << 8) | *(data + 7);
+    timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
+                  (*(data + 10) << 8) | *(data + 11);
+    timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
     flag = *(data + 12);
     temperature = *((float*)(data+ 13));  // NOLINT
     // LOG(INFO) << "temperature:" << temperature;
@@ -87,6 +89,11 @@ struct ImuData2 {
     // LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
     accel_or_gyro[2] = *((float*)(data + 25));  // NOLINT
     // LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2];
+    if (flag == 3) {
+      gyro_add[0] = *((float*)(data + 29));  // NOLINT
+      gyro_add[1] = *((float*)(data + 33));  // NOLINT
+      gyro_add[2] = *((float*)(data + 37));  // NOLINT
+    }
   }
 };
 
@@ -113,7 +120,6 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
   seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
   seg->temperature = imu.temperature;
   if (seg->flag == 1) {
-    // LOG(INFO) << "flag1";
     seg->accel[0] = imu.accel_or_gyro[0];
     seg->accel[1] = imu.accel_or_gyro[1];
     seg->accel[2] = imu.accel_or_gyro[2];
@@ -121,7 +127,6 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
     seg->gyro[1] = 0.;
     seg->gyro[2] = 0.;
   } else if (seg->flag == 2) {
-    // LOG(INFO) << "flag2";
     seg->gyro[0] = imu.accel_or_gyro[0];
     seg->gyro[1] = imu.accel_or_gyro[1];
     seg->gyro[2] = imu.accel_or_gyro[2];
@@ -130,12 +135,12 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
     seg->accel[2] = 0.;
   } else if (seg->flag == 3) {
     LOG(INFO) << "flag3";
-    // seg->gyro[0] = imu.accel_or_gyro[0];
-    // seg->gyro[1] = imu.accel_or_gyro[1];
-    // seg->gyro[2] = imu.accel_or_gyro[2];
-    // seg->accel[0] = 0.;
-    // seg->accel[1] = 0.;
-    // seg->accel[2] = 0.;
+    seg->gyro[0] = imu.accel_or_gyro[0];
+    seg->gyro[1] = imu.accel_or_gyro[1];
+    seg->gyro[2] = imu.accel_or_gyro[2];
+    seg->accel[0] = imu.gyro_add[0];
+    seg->accel[1] = imu.gyro_add[1];
+    seg->accel[2] = imu.gyro_add[2];
   }
 }
 
@@ -154,8 +159,12 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
   }
 }
 
-void unpack_imu_packet2(const std::uint8_t *data, ImuPacket2 *pkg) {
-  std::size_t data_n = sizeof(ImuData2);  // 29
+void unpack_imu_packet2(
+    const std::uint8_t *data, ImuPacket2 *pkg, bool is_correspondence_on) {
+  std::size_t data_n = 29;
+  if (is_correspondence_on) {
+    data_n = 41;
+  }
   for (std::size_t i = 0; i < pkg->count; i++) {
     ImuSegment2 seg;
     unpack_imu_segment2(ImuData2(data + data_n * i), &seg);
@@ -182,16 +191,20 @@ void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
   res->checksum = *(data + 4 + res->size);
 }
 
-void unpack_imu_res_packet2(const std::uint8_t *data, ImuResPacket2 *res) {
+void unpack_imu_res_packet2(
+    const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
   res->header = *data;
   // u_int64_t* jj = (u_int64_t*) data;
   res->state = *(data + 1);
   res->size = (*(data + 2) << 8) | *(data + 3);
-  std::size_t data_n = sizeof(ImuData2);  // 21
+  std::size_t data_n = 29;
+  if (is_correspondence_on) {
+    data_n = 41;
+  }
   // LOG(INFO) << "size:" << data_n;
   ImuPacket2 packet;
   packet.count = res->size / data_n;
-  unpack_imu_packet2(data + 4, &packet);
+  unpack_imu_packet2(data + 4, &packet, is_correspondence_on);
   res->packets.push_back(packet);
   res->checksum = *(data + 4 + res->size);
 }
@@ -227,8 +240,8 @@ void Standard2ChannelsAdapter::GetImuResPacket(
 }
 
 void Standard2ChannelsAdapter::GetImuResPacket2(
-    const std::uint8_t *data, ImuResPacket2 *res) {
-  unpack_imu_res_packet2(data, res);
+    const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
+  unpack_imu_res_packet2(data, res, is_correspondence_on);
 }
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.h b/src/mynteye/device/standard2/channels_adapter_s2.h
index 64510b1..c085052 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.h
+++ b/src/mynteye/device/standard2/channels_adapter_s2.h
@@ -35,7 +35,10 @@ class Standard2ChannelsAdapter : public ChannelsAdapter {
   std::vector<std::int32_t> GetGyroRangeValues() override;
 
   void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
-  void GetImuResPacket2(const std::uint8_t *data, ImuResPacket2 *res) override;
+  void GetImuResPacket2(
+      const std::uint8_t *data,
+      ImuResPacket2 *res,
+      bool is_correspondence_on) override;
 };
 
 MYNTEYE_END_NAMESPACE

From 78fe5c813de19613d57d94e677a8306add7f0c98 Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Sun, 29 Sep 2019 16:46:40 +0800
Subject: [PATCH 5/5] style(imu): remove useless log.

---
 src/mynteye/device/channel/channels.cc              | 6 +-----
 src/mynteye/device/standard2/channels_adapter_s2.cc | 1 -
 2 files changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc
index 4134e03..5f7197d 100644
--- a/src/mynteye/device/channel/channels.cc
+++ b/src/mynteye/device/channel/channels.cc
@@ -879,11 +879,7 @@ bool Channels::XuImuRead(ImuResPacket2 *res) const {
                   << " now";
       return false;
     }
-    // LOG(INFO) << res->size;
-    if (enable_imu_correspondence) {
-      LOG(INFO) << "enable_imu_correspondence";
-    } else {
-    }
+
     VLOG(2) << "XuImuRead response success";
     return true;
   } else {
diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc
index 4e6ba06..2f9d2b1 100644
--- a/src/mynteye/device/standard2/channels_adapter_s2.cc
+++ b/src/mynteye/device/standard2/channels_adapter_s2.cc
@@ -134,7 +134,6 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
     seg->accel[1] = 0.;
     seg->accel[2] = 0.;
   } else if (seg->flag == 3) {
-    LOG(INFO) << "flag3";
     seg->gyro[0] = imu.accel_or_gyro[0];
     seg->gyro[1] = imu.accel_or_gyro[1];
     seg->gyro[2] = imu.accel_or_gyro[2];