From 366c68a8c9f48e89fa39b6bbacf2907d27d329d0 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 12 Apr 2018 20:21:40 +0800 Subject: [PATCH] Fix imu response packet --- doc/zh-Hans/spec_imu_data.md | 4 ++-- src/internal/channels.cc | 34 +++++++++++++++++++++++++++------- src/internal/motions.cc | 8 ++++---- src/internal/types.h | 33 ++++++++++++++++++++------------- 4 files changed, 53 insertions(+), 26 deletions(-) diff --git a/doc/zh-Hans/spec_imu_data.md b/doc/zh-Hans/spec_imu_data.md index 7549790..aa6aecc 100644 --- a/doc/zh-Hans/spec_imu_data.md +++ b/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** 。 diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 63bf428..9950633 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -223,26 +223,46 @@ 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(); } }); } diff --git a/src/internal/motions.cc b/src/internal/motions.cc index e8da952..2fbc680 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -33,10 +33,10 @@ void Motions::StartMotionTracking() { for (auto &&seg : packet.segments) { auto &&imu = std::make_shared(); imu->frame_id = seg.frame_id; - if (seg.offset < 0 && - static_cast(-seg.offset) > packet.timestamp) { - LOG(WARNING) << "Imu timestamp offset is incorrect"; - } + // if (seg.offset < 0 && + // static_cast(-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; diff --git a/src/internal/types.h b/src/internal/types.h index 63306c5..dd76ec2 100644 --- a/src/internal/types.h +++ b/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 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); } };