feat(imu): imu2.0 decond part complete.

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

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

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

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

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/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.";
 }
 

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

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[0] = imu.accel_or_gyro[0];
-    // seg->gyro[1] = imu.accel_or_gyro[1];
+    seg->gyro[1] = imu.accel_or_gyro[1];
-    // seg->gyro[2] = imu.accel_or_gyro[2];
+    seg->gyro[2] = imu.accel_or_gyro[2];
-    // seg->accel[0] = 0.;
+    seg->accel[0] = imu.gyro_add[0];
-    // seg->accel[1] = 0.;
+    seg->accel[1] = imu.gyro_add[1];
-    // seg->accel[2] = 0.;
+    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) {
+void unpack_imu_packet2(
-  std::size_t data_n = sizeof(ImuData2);  // 29
+    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) {
+    const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
-  unpack_imu_res_packet2(data, res);
+  unpack_imu_res_packet2(data, res, is_correspondence_on);
 }
 
 MYNTEYE_END_NAMESPACE

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