From 0d664b28ad7260e4497822c73fabf17aff79da98 Mon Sep 17 00:00:00 2001 From: kalman Date: Fri, 21 Dec 2018 22:04:40 +0800 Subject: [PATCH] fix(imu):fix tbe imu bug of s1030 --- include/mynteye/types.h | 4 +- src/mynteye/device/channels.cc | 4 +- src/mynteye/device/channels.h | 2 + src/mynteye/device/motions.cc | 31 ++--- src/mynteye/device/motions.h | 2 +- src/mynteye/device/standard/device_s.cc | 2 +- src/mynteye/device/standard2/device_s2.cc | 2 +- src/mynteye/device/types.h | 152 ++++++++++++++++++---- 8 files changed, 147 insertions(+), 52 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 319c56b..d704b97 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -515,8 +515,8 @@ struct MYNTEYE_API ImgData { * IMU data. */ struct MYNTEYE_API ImuData { - /** Imu serial number */ - std::uint32_t serial_number; + /** Imu frame id */ + std::uint32_t frame_id; /** accel or gyro flag:1 for accel,2 for gyro,3 for both */ std::uint8_t flag; /** IMU timestamp in 1us */ diff --git a/src/mynteye/device/channels.cc b/src/mynteye/device/channels.cc index 68e74c0..30de25b 100644 --- a/src/mynteye/device/channels.cc +++ b/src/mynteye/device/channels.cc @@ -333,7 +333,7 @@ void Channels::SetImuCallback(imu_callback_t callback) { void Channels::DoImuTrack() { static ImuReqPacket req_packet{0}; - static ImuResPacket res_packet; + static ImuResPacket res_packet(model_); req_packet.serial_number = imu_sn_; if (!XuImuWrite(req_packet)) { @@ -360,7 +360,7 @@ void Channels::DoImuTrack() { return n; }(); - auto &&sn = res_packet.packets.back().segments.back().serial_number; + auto &&sn = res_packet.packets.back().serial_number; if (imu_sn_ == sn) { VLOG(2) << "New imu not ready, dropped"; return; diff --git a/src/mynteye/device/channels.h b/src/mynteye/device/channels.h index 3c42c65..b55af5d 100644 --- a/src/mynteye/device/channels.h +++ b/src/mynteye/device/channels.h @@ -71,6 +71,8 @@ class MYNTEYE_API Channels { using img_params_t = std::map; using imu_params_t = device::imu_params_t; + std::uint8_t model_; + explicit Channels(std::shared_ptr device); ~Channels(); diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index 3020196..c7d2e6e 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -36,7 +36,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) { if (motion_callback_) { accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE); if (accel_range == -1) - accel_range = 12; + accel_range = (channels_->model_) ? 8 : 12; // ugly gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE); if (gyro_range == -1) @@ -53,28 +53,16 @@ void Motions::SetMotionCallback(motion_callback_t callback) { // static_cast(-seg.offset) > packet.timestamp) { // LOG(WARNING) << "Imu timestamp offset is incorrect"; // } - imu->serial_number = seg.serial_number; + imu->frame_id = seg.frame_id; imu->timestamp = seg.timestamp; imu->flag = seg.flag; imu->temperature = seg.temperature / 326.8f + 25; - - if (imu->flag == 1) { - imu->accel[0] = seg.accel_or_gyro[0] * 1.f * accel_range / 0x10000; - imu->accel[1] = seg.accel_or_gyro[1] * 1.f * accel_range / 0x10000; - imu->accel[2] = seg.accel_or_gyro[2] * 1.f * accel_range / 0x10000; - imu->gyro[0] = 0; - imu->gyro[1] = 0; - imu->gyro[2] = 0; - } else if (imu->flag == 2) { - imu->accel[0] = 0; - imu->accel[1] = 0; - imu->accel[2] = 0; - imu->gyro[0] = seg.accel_or_gyro[0] * 1.f * gyro_range / 0x10000; - imu->gyro[1] = seg.accel_or_gyro[1] * 1.f * gyro_range / 0x10000; - imu->gyro[2] = seg.accel_or_gyro[2] * 1.f * gyro_range / 0x10000; - } else { - imu->Reset(); - } + imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000; + imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000; + imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000; + imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000; + imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000; + imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000; std::lock_guard _(mtx_datas_); motion_data_t data = {imu}; @@ -90,7 +78,8 @@ void Motions::SetMotionCallback(motion_callback_t callback) { } } -void Motions::DoMotionTrack() { +void Motions::DoMotionTrack(std::uint8_t model) { + channels_->model_ = model; channels_->DoImuTrack(); } diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h index 7760e0d..129727b 100644 --- a/src/mynteye/device/motions.h +++ b/src/mynteye/device/motions.h @@ -37,7 +37,7 @@ class Motions { ~Motions(); void SetMotionCallback(motion_callback_t callback); - void DoMotionTrack(); + void DoMotionTrack(std::uint8_t model); void StartMotionTracking(); void StopMotionTracking(); diff --git a/src/mynteye/device/standard/device_s.cc b/src/mynteye/device/standard/device_s.cc index 36719ba..7c4fddd 100644 --- a/src/mynteye/device/standard/device_s.cc +++ b/src/mynteye/device/standard/device_s.cc @@ -44,7 +44,7 @@ void StandardDevice::StartVideoStreaming() { void StandardDevice::OnStereoStreamUpdate() { if (motion_tracking_) { auto &&motions = this->motions(); - motions->DoMotionTrack(); + motions->DoMotionTrack(1); } } diff --git a/src/mynteye/device/standard2/device_s2.cc b/src/mynteye/device/standard2/device_s2.cc index 4b5fc44..96f99d6 100644 --- a/src/mynteye/device/standard2/device_s2.cc +++ b/src/mynteye/device/standard2/device_s2.cc @@ -39,7 +39,7 @@ std::shared_ptr Standard2Device::CreateStreamsAdapter() const { void Standard2Device::OnStereoStreamUpdate() { if (motion_tracking_) { auto &&motions = this->motions(); - motions->DoMotionTrack(); + motions->DoMotionTrack(2); } } diff --git a/src/mynteye/device/types.h b/src/mynteye/device/types.h index b056b06..f04d09c 100644 --- a/src/mynteye/device/types.h +++ b/src/mynteye/device/types.h @@ -140,7 +140,7 @@ struct MYNTEYE_API DeviceInfo { /** * @ingroup datatypes - * Image packet for standand 2. + * Image packet for standard 2. */ #pragma pack(push, 1) struct ImagePacketS2 { @@ -176,7 +176,7 @@ struct ImagePacketS2 { /** * @ingroup datatypes - * Image packet for standand 1. + * Image packet for standard 1. */ #pragma pack(push, 1) struct ImagePacketS1 { @@ -230,18 +230,49 @@ struct ImuReqPacket { /** * @ingroup datatypes - * Imu segment. + * Imu group for standard 1. */ #pragma pack(push, 1) -struct ImuSegment { - std::uint32_t serial_number; +struct ImuGroupS1 { + std::int16_t offset; + std::uint16_t frame_id; + std::int16_t accel[3]; + std::int16_t temperature; + std::int16_t gyro[3]; + + ImuGroupS1() = default; + explicit ImuGroupS1(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) << 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) + +/** + * @ingroup datatypes + * Imu group for standard 2. + */ +#pragma pack(push, 1) +struct ImuGroupS2 { + std::uint32_t frame_id; std::uint64_t timestamp; std::uint8_t flag; std::int16_t temperature; std::int16_t accel_or_gyro[3]; - ImuSegment() = default; - explicit ImuSegment(std::uint8_t *data) { + ImuGroupS2() = default; + explicit ImuGroupS2(std::uint8_t *data) { from_data(data); } @@ -249,7 +280,7 @@ struct ImuSegment { std::uint32_t timestamp_l; std::uint32_t timestamp_h; - serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) | + 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); @@ -265,24 +296,86 @@ struct ImuSegment { }; #pragma pack(pop) +/** + * @ingroup datatypes + * Imu segment. + */ +#pragma pack(push, 1) +struct ImuSegment { + std::uint32_t frame_id; + std::uint64_t timestamp; + std::uint8_t flag; + std::int16_t temperature; + std::int16_t accel[3]; + std::int16_t gyro[3]; + + ImuSegment() = default; + + explicit ImuSegment(ImuGroupS2 group) + : frame_id(group.frame_id), timestamp(group.timestamp), + flag(group.flag), temperature(group.temperature) { + accel[0] = (flag == 1)?group.accel_or_gyro[0]:0; + accel[1] = (flag == 1)?group.accel_or_gyro[1]:0; + accel[2] = (flag == 1)?group.accel_or_gyro[2]:0; + gyro[0] = (flag == 2)?group.accel_or_gyro[0]:0; + gyro[1] = (flag == 2)?group.accel_or_gyro[1]:0; + gyro[2] = (flag == 2)?group.accel_or_gyro[2]:0; + } + + explicit ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) { + frame_id = static_cast (group.frame_id); + this->timestamp = static_cast (timestamp + group.offset); + flag = 3; + temperature = group.temperature; + accel[0] = group.accel[0]; + accel[1] = group.accel[1]; + accel[2] = group.accel[2]; + gyro[0] = group.gyro[0]; + gyro[1] = group.gyro[1]; + gyro[2] = group.gyro[2]; + } +}; +#pragma pack(pop) + /** * @ingroup datatypes * Imu packet. */ #pragma pack(push, 1) struct ImuPacket { + std::uint8_t model; std::uint8_t count; + std::uint32_t serial_number; std::vector segments; ImuPacket() = default; - explicit ImuPacket(std::uint8_t seg_count, std::uint8_t *data) { - count = seg_count; - from_data(data); - } + explicit ImuPacket( + std::uint8_t model, std::uint8_t seg_count, std::uint8_t *data) + : model(model), count(seg_count) { + from_data(data); + } void from_data(std::uint8_t *data) { - std::size_t seg_n = sizeof(ImuSegment); // 21 - for (std::size_t i = 0; i < count; i++) { - segments.push_back(ImuSegment(data + seg_n * i)); + if (model == 1) { + serial_number = + (*(data) << 24) | (*(data + 1) << 16) | + (*(data + 2) << 8) | *(data + 3); + int timestamp = + (*(data + 4) << 24) | (*(data + 5) << 16)| + (*(data + 6) << 8) | *(data + 7); + count = *(data + 8); + + std::size_t seg_n = sizeof(ImuGroupS1); // 18 + for (std::size_t i = 0; i < count; i++) { + ImuGroupS1 group(data + 9 + (seg_n * i)); + segments.push_back(ImuSegment(timestamp, group)); + } + } else if (model == 2) { + std::size_t seg_n = sizeof(ImuGroupS2); // 21 + for (std::size_t i = 0; i < count; i++) { + ImuGroupS2 group(data + seg_n * i); + segments.push_back(ImuSegment(group)); + } + serial_number = segments.back().frame_id; } } }; @@ -294,6 +387,7 @@ struct ImuPacket { */ #pragma pack(push, 1) struct ImuResPacket { + std::uint8_t model; std::uint8_t header; std::uint8_t state; std::uint16_t size; @@ -301,21 +395,31 @@ struct ImuResPacket { std::uint8_t checksum; ImuResPacket() = default; - explicit ImuResPacket(std::uint8_t *data) { - from_data(data); - } + explicit ImuResPacket(std::uint8_t model) : model(model) {} void from_data(std::uint8_t *data) { header = *data; state = *(data + 1); size = (*(data + 2) << 8) | *(data + 3); - std::size_t seg_n = sizeof(ImuSegment); // 21 - std::uint8_t seg_count = size / seg_n; - ImuPacket packet(seg_count, data + 4); - packets.push_back(packet); - // packet(2); - checksum = *(data + 4 + size); + if (model == 1) { + std::size_t seg_n = sizeof(ImuGroupS1); // 18 + for (std::size_t i = 4; i < size;) { + ImuPacket packet(model, 0, data + i); + packets.push_back(packet); + i += 9 + (packet.count * seg_n); + } + checksum = *(data + 4 + size); + } + + if (model == 2) { + std::size_t seg_n = sizeof(ImuGroupS2); // 21 + std::uint8_t seg_count = size / seg_n; + ImuPacket packet(model, seg_count, data + 4); + packets.push_back(packet); + // packet(2); + checksum = *(data + 4 + size); + } } }; #pragma pack(pop)