Add imu write & read

doc/zh-Hans/spec_imu_data.md

@@ -32,7 +32,7 @@ IMU 包/小包，是一组 IMU 数据。
 
 | Name | Offset | FrameID | Accelerometer | Temperature | Gyroscope |
 | :--- | :----- | :------ | :------------ | :---------- | :-------- |
-| 字节数 | 2 | 2 | 6 | 2 | 6 |
+| 字节数 | 2 | 2 | 3 | 1 | 3 |
 | 类型 | int16_t | uint16_t | int8_t * 3 | int8_t | int8_t * 3 |
 | Description | 相对基准时间戳的偏移量 | 图像帧 ID | 加速度计 x y z 三轴的值 | IMU 的温度 | 陀螺仪 x y z 三轴的值 |
 

src/internal/channels.cc

@@ -108,11 +108,11 @@ std::int32_t Channels::GetControlValue(const Option &option) const {
     case Option::BRIGHTNESS:
     case Option::CONTRAST:
       std::int32_t value;
-      if (!PuControlQuery(option, uvc::PU_QUERY_GET, &value)) {
+      if (PuControlQuery(option, uvc::PU_QUERY_GET, &value)) {
+        return value;
+      } else {
         LOG(WARNING) << option << " get value failed";
         return -1;
-      } else {
-        return value;
       }
     case Option::FRAME_RATE:
     case Option::IMU_FREQUENCY:
@@ -251,12 +251,12 @@ void Channels::XuCamCtrlSet(Option option, std::int32_t value) const {
   std::uint8_t data[3] = {static_cast<std::uint8_t>(id & 0xFF),
                           static_cast<std::uint8_t>((value >> 8) & 0xFF),
                           static_cast<std::uint8_t>(value & 0xFF)};
-  if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
+  if (XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
-    LOG(WARNING) << "XuCamCtrlSet value (" << value << ") of " << option
-                 << " failed";
-  } else {
     VLOG(2) << "XuCamCtrlSet value (" << value << ") of " << option
             << " success";
+  } else {
+    LOG(WARNING) << "XuCamCtrlSet value (" << value << ") of " << option
+                 << " failed";
   }
 }
 
@@ -264,14 +264,39 @@ bool Channels::XuHalfDuplexSet(Option option, xu_cmd_t cmd) const {
   int id = XuHalfDuplexId(option);
   std::uint8_t data[3] = {// must be 3 now
                           static_cast<std::uint8_t>(id & 0xFF), cmd};
-  if (!XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 3, data)) {
+  if (XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 3, data)) {
-    LOG(WARNING) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase
-                 << cmd << ") of " << option << " failed";
-    return false;
-  } else {
     VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << cmd
             << ") of " << option << " success";
     return true;
+  } else {
+    LOG(WARNING) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase
+                 << cmd << ") of " << option << " failed";
+    return false;
+  }
+}
+
+bool Channels::XuImuWrite(const ImuReqPacket &req) const {
+  auto &&data = req.to_data();
+  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(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)) {
+    VLOG(2) << "XuImuRead response success";
+    res->from_data(data);
+    return true;
+  } else {
+    LOG(WARNING) << "XuImuRead response failed";
+    return false;
   }
 }
 

src/internal/channels.h

@@ -6,6 +6,8 @@
 #include <memory>
 
 #include "mynteye/mynteye.h"
+
+#include "internal/types.h"
 #include "uvc/uvc.h"
 
 MYNTEYE_BEGIN_NAMESPACE
@@ -70,6 +72,9 @@ class Channels {
 
   bool XuHalfDuplexSet(Option option, xu_cmd_t cmd) const;
 
+  bool XuImuWrite(const ImuReqPacket &req) const;
+  bool XuImuRead(ImuResPacket *res) const;
+
   control_info_t PuControlInfo(Option option) const;
   control_info_t XuControlInfo(Option option) const;
 

src/internal/types.h

@@ -4,6 +4,7 @@
 
 #include <cstdint>
 
+#include <array>
 #include <bitset>
 #include <string>
 #include <vector>
@@ -140,6 +141,10 @@ struct ImagePacket {
 
   ImagePacket() = default;
   explicit ImagePacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
     header = *data;
     size = *(data + 1);
     frame_id = (*(data + 2) << 8) + *(data + 3);
@@ -151,6 +156,119 @@ struct ImagePacket {
 };
 #pragma pack(pop)
 
+/**
+ * @ingroup datatypes
+ * Imu request packet.
+ */
+#pragma pack(push, 1)
+struct ImuReqPacket {
+  std::uint8_t header;
+  std::uint32_t serial_number;
+
+  ImuReqPacket() = default;
+  explicit ImuReqPacket(std::uint32_t serial_number)
+      : ImuReqPacket(0x5A, serial_number) {}
+  ImuReqPacket(std::uint8_t header, std::uint32_t serial_number)
+      : header(header), serial_number(serial_number) {}
+
+  std::array<std::uint8_t, 5> to_data() const {
+    return {header, static_cast<std::uint8_t>((serial_number >> 24) & 0xFF),
+            static_cast<std::uint8_t>((serial_number >> 16) & 0xFF),
+            static_cast<std::uint8_t>((serial_number >> 8) & 0xFF),
+            static_cast<std::uint8_t>(serial_number & 0xFF)};
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu segment.
+ */
+#pragma pack(push, 1)
+struct ImuSegment {
+  std::int16_t offset;
+  std::uint16_t frame_id;
+  std::int8_t accel[3];
+  std::int8_t temperature;
+  std::int8_t gyro[3];
+
+  ImuSegment() = default;
+  explicit ImuSegment(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    offset = (*(data) << 8) + *(data + 1);
+    frame_id = (*(data + 2) << 8) + *(data + 3);
+    accel[0] = *(data + 4);
+    accel[1] = *(data + 5);
+    accel[2] = *(data + 6);
+    temperature = *(data + 7);
+    gyro[0] = *(data + 8);
+    gyro[1] = *(data + 9);
+    gyro[2] = *(data + 10);
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu packet.
+ */
+#pragma pack(push, 1)
+struct ImuPacket {
+  std::uint32_t serial_number;
+  std::uint32_t timestamp;
+  std::uint8_t count;
+  std::vector<ImuSegment> segments;
+
+  ImuPacket() = default;
+  explicit ImuPacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    serial_number = (*(data) << 24) + (*(data + 1) << 16) + (*(data + 2) << 8) +
+                    *(data + 3);
+    timestamp = (*(data + 4) << 24) + (*(data + 5) << 16) + (*(data + 6) << 8) +
+                *(data + 7);
+    count = *(data + 8);
+
+    std::size_t seg_n = sizeof(ImuSegment);
+    for (std::size_t i = 0; i < count; i++) {
+      segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
+    }
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu response packet.
+ */
+#pragma pack(push, 1)
+struct ImuResPacket {
+  std::uint8_t header;
+  std::uint8_t state;
+  std::uint16_t size;
+  ImuPacket packet;
+  std::uint8_t checksum;
+
+  ImuResPacket() = default;
+  explicit ImuResPacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    header = *data;
+    state = *(data + 1);
+    size = (*(data + 2) << 8) + *(data + 3);
+    packet.from_data(data + 4);
+    checksum = *(data + 4 + size);
+  }
+};
+#pragma pack(pop)
+
 #undef MYNTEYE_PROPERTY
 
 MYNTEYE_END_NAMESPACE