Merge branch 'imu_2.0_protocl' into develop

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();
 };
 

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.
    */

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);
 

samples/util_cv.cc

@@ -167,23 +167,8 @@ cv::Rect CVPainter::DrawImuData(
       img, ss.str(), gravity, 5, 0,
       sign * (10 + rect_i.height + rect_a.height));
 
-  static double gyro1_s = 0.0;
+  Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
-  static double gyro2_s = 0.0;
+            << data.gyro[1] << "," << fmt_imu << data.gyro[2];
-  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;
   cv::Rect rect_g = DrawText(
       img, ss.str(), gravity, 5, 0,
       sign * (15 + rect_i.height + rect_a.height + rect_p.height));

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;

src/mynteye/device/channel/channels.cc

@@ -30,6 +30,32 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
+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] * 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 / 326.8f + 25;
+    tpr.timestamp = pak1.segments[i].timestamp;
+    res.segments.push_back(tpr);
+  }
+  return res;
+}
+
+
 namespace {
 
 const uvc::xu mynteye_xu = {3, 2,
@@ -112,12 +138,20 @@ 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),
     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() {
@@ -328,7 +362,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 +413,7 @@ void Channels::SetImuCallback(imu_callback_t callback) {
 }
 
 void Channels::DoImuTrack() {
-  if (IsImuProc2()) {
+  if (IsImuProtocol2()) {
     return DoImuTrack2();
   } else {
     return DoImuTrack1();
@@ -424,7 +458,7 @@ void Channels::DoImuTrack1() {
 
   if (imu_callback_) {
     for (auto &&packet : res_packet.packets) {
-      imu_callback_(packet);
+      imu_callback_(to_pak2(packet, accel_range, gyro_range));
     }
   }
 
@@ -432,49 +466,41 @@ void Channels::DoImuTrack1() {
 }
 
 void Channels::DoImuTrack2() {
-  // static ImuReqPacket req_packet{0};
+  // LOG(INFO) << "wait to adapter!";
-  // static ImuResPacket res_packet;
+  static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
+  static ImuResPacket2 res_packet;
+  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);
+    }
+  }
 
-  // req_packet.serial_number = imu_sn_;
+  res_packet.packets.clear();
-  // 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!";
 }
 
 
@@ -499,7 +525,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 +827,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 +839,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,12 +850,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, 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)
+                  << 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;
+    }
+
+    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
   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)

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,8 +139,11 @@ 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_;
+  int accel_range;
+  int gyro_range;
 
   std::uint32_t imu_sn_;
   imu_callback_t imu_callback_;
@@ -157,6 +165,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, bool is_correspondence_on) = 0;
 
  protected:
   Model model_;

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;
+  float temperature;
+  float accel[3];
+  float 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_

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>();
 

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;
       }
@@ -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->temperature = seg.temperature / 326.8f + 25;
-        imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
+        imu->temperature = seg.temperature;
-        imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
+        // LOG(INFO) << "beforea" << seg.accel[0];
-        imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
+        // LOG(INFO) << "before" << seg.accel[1];
-        imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
+        // LOG(INFO) << "before" << seg.accel[2];
-        imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
+        // LOG(INFO) << "beforeg" << seg.gyro[0];
-        imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
+        // 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);

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, bool is_correspondence_on) {
+  LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/standard/channels_adapter_s.h

@@ -35,6 +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,
+      bool is_correspondence_on) override;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/device/standard2/channels_adapter_s2.cc

@@ -52,6 +52,53 @@ struct ImuData {
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
+
+#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];
+  float gyro_add[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 = BYTE_4(data, 0);
+    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;
+    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];
+    if (flag == 3) {
+      gyro_add[0] = *((float*)(data + 29));  // NOLINT
+      gyro_add[1] = *((float*)(data + 33));  // NOLINT
+      gyro_add[2] = *((float*)(data + 37));  // NOLINT
+    }
+  }
+};
+
+#pragma pack(pop)
+
 void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
   seg->frame_id = imu.frame_id;
   seg->timestamp = imu.timestamp;
@@ -66,6 +113,36 @@ 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) {
+  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;
+  if (seg->flag == 1) {
+    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) {
+    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) {
+    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];
+  }
+}
+
 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 +158,25 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
   }
 }
 
+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);
+    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 +190,24 @@ 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, 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 = 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, is_correspondence_on);
+  res->packets.push_back(packet);
+  res->checksum = *(data + 4 + res->size);
+}
+
 }  // namespace
 
 Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
@@ -124,4 +238,9 @@ void Standard2ChannelsAdapter::GetImuResPacket(
   unpack_imu_res_packet(data, res);
 }
 
+void Standard2ChannelsAdapter::GetImuResPacket2(
+    const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
+  unpack_imu_res_packet2(data, res, is_correspondence_on);
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/standard2/channels_adapter_s2.h

@@ -35,6 +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,
+      bool is_correspondence_on) override;
 };
 
 MYNTEYE_END_NAMESPACE