Fix imu response packet

doc/zh-Hans/spec_imu_data.md

@@ -32,8 +32,8 @@ IMU 包/小包，是一组 IMU 数据。
 
 | Name | Offset | FrameID | Accelerometer | Temperature | Gyroscope |
 | :--- | :----- | :------ | :------------ | :---------- | :-------- |
-| 字节数 | 2 | 2 | 3 | 1 | 3 |
-| 类型 | int16_t | uint16_t | int8_t * 3 | int8_t | int8_t * 3 |
+| 字节数 | 2 | 2 | 6 | 2 | 6 |
+| 类型 | int16_t | uint16_t | int16_t * 3 | int16_t | int16_t * 3 |
 | Description | 相对基准时间戳的偏移量 | 图像帧 ID | 加速度计 x y z 三轴的值 | IMU 的温度 | 陀螺仪 x y z 三轴的值 |
 
 * 加速度计和陀螺仪的计量值换算成物理值公式： **real = data * range / 0x10000** 。

src/internal/channels.cc

@@ -223,27 +223,47 @@ void Channels::StartImuTracking(imu_callback_t callback) {
     imu_sn_ = 0;
     ImuReqPacket req_packet{imu_sn_};
     ImuResPacket res_packet;
-    auto sleep_milli = [](std::intmax_t n) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(n));
-    };
+    // auto sleep_milli = [](std::intmax_t n) {
+    //   std::this_thread::sleep_for(std::chrono::milliseconds(n));
+    // };
     while (!imu_track_stop_) {
       req_packet.serial_number = imu_sn_;
       if (!XuImuWrite(req_packet)) {
-        sleep_milli(5);
         continue;
       }
 
       if (!XuImuRead(&res_packet)) {
-        sleep_milli(5);
         continue;
       }
 
-      imu_sn_ = res_packet.packet.serial_number;
+      if (res_packet.packets.size() == 0) {
+        continue;
+      }
+
+      VLOG(2) << "Imu req sn: " << imu_sn_
+              << ", res count: " << [&res_packet]() {
+                   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";
+        continue;
+      }
+      imu_sn_ = sn;
 
       if (imu_callback_) {
-        imu_callback_(res_packet.packet);
+        for (auto &&packet : res_packet.packets) {
+          imu_callback_(packet);
         }
       }
+
+      res_packet.packets.clear();
+    }
   });
 }
 

src/internal/motions.cc

@@ -33,10 +33,10 @@ void Motions::StartMotionTracking() {
       for (auto &&seg : packet.segments) {
         auto &&imu = std::make_shared<ImuData>();
         imu->frame_id = seg.frame_id;
-        if (seg.offset < 0 &&
-            static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
-          LOG(WARNING) << "Imu timestamp offset is incorrect";
-        }
+        // if (seg.offset < 0 &&
+        //     static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
+        //   LOG(WARNING) << "Imu timestamp offset is incorrect";
+        // }
         imu->timestamp = packet.timestamp + seg.offset;
         imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
         imu->accel[1] = seg.accel[1] * 8.f / 0x10000;

src/internal/types.h

@@ -188,9 +188,9 @@ struct ImuReqPacket {
 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];
+  std::int16_t accel[3];
+  std::int16_t temperature;
+  std::int16_t gyro[3];
 
   ImuSegment() = default;
   explicit ImuSegment(std::uint8_t *data) {
@@ -200,13 +200,13 @@ struct ImuSegment {
   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);
+    accel[0] = (*(data + 4) << 8) + *(data + 5);
+    accel[1] = (*(data + 6) << 8) + *(data + 7);
+    accel[2] = (*(data + 8) << 8) + *(data + 9);
+    temperature = (*(data + 10) << 8) + *(data + 11);
+    gyro[0] = (*(data + 12) << 8) + *(data + 13);
+    gyro[1] = (*(data + 14) << 8) + *(data + 15);
+    gyro[2] = (*(data + 16) << 8) + *(data + 17);
   }
 };
 #pragma pack(pop)
@@ -234,7 +234,7 @@ struct ImuPacket {
                 *(data + 7);
     count = *(data + 8);
 
-    std::size_t seg_n = sizeof(ImuSegment);
+    std::size_t seg_n = sizeof(ImuSegment);  // 18
     for (std::size_t i = 0; i < count; i++) {
       segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
     }
@@ -251,7 +251,7 @@ struct ImuResPacket {
   std::uint8_t header;
   std::uint8_t state;
   std::uint16_t size;
-  ImuPacket packet;
+  std::vector<ImuPacket> packets;
   std::uint8_t checksum;
 
   ImuResPacket() = default;
@@ -263,7 +263,14 @@ struct ImuResPacket {
     header = *data;
     state = *(data + 1);
     size = (*(data + 2) << 8) + *(data + 3);
-    packet.from_data(data + 4);
+
+    std::size_t seg_n = sizeof(ImuSegment);  // 18
+    for (std::size_t i = 4; i < size;) {
+      ImuPacket packet(data + i);
+      packets.push_back(packet);
+      i += 9 + (packet.count * seg_n);
+    }
+
     checksum = *(data + 4 + size);
   }
 };