feat(imu): imu 2.0 protocl.

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};
+  // LOG(INFO) << "wait to adapter!";
-  // static ImuResPacket res_packet;
+  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_;
+  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 +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

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

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_

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

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

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

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

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