From 7e0e4b83e72fc6205a6d89a1d571d85b8be5f48e Mon Sep 17 00:00:00 2001 From: TinyO Date: Sun, 29 Sep 2019 15:11:16 +0800 Subject: [PATCH] feat(imu): imu2.0 first complete. --- samples/util_cv.cc | 19 +----- src/mynteye/device/channel/channels.cc | 45 ++++++++------ src/mynteye/device/channel/channels.h | 2 + src/mynteye/device/channel/def.h | 6 +- src/mynteye/device/motions.cc | 35 ++++++++--- .../device/standard2/channels_adapter_s2.cc | 61 +++++++++++-------- 6 files changed, 97 insertions(+), 71 deletions(-) diff --git a/samples/util_cv.cc b/samples/util_cv.cc index 1911d78..6e4b547 100644 --- a/samples/util_cv.cc +++ b/samples/util_cv.cc @@ -141,23 +141,8 @@ cv::Rect CVPainter::DrawImuData( cv::Rect rect_a = DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height)); - static double gyro1_s = 0.0; - static double gyro2_s = 0.0; - static double gyro3_s = 0.0; - - if (data.gyro[0] > 0.01 || - data.gyro[1] > 0.01 || - data.gyro[2] > 0.01 || - data.gyro[0] < -0.01 || - data.gyro[1] < -0.01 || - data.gyro[2] < -0.01 ) { - gyro1_s = data.gyro[0]; - gyro2_s = data.gyro[1]; - gyro3_s = data.gyro[2]; - } - - Clear(ss) << "gyro(x,y,z): " << fmt_imu << gyro1_s << "," << fmt_imu - << gyro2_s << "," << fmt_imu << gyro3_s; + Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu + << data.gyro[1] << "," << fmt_imu << data.gyro[2]; cv::Rect rect_g = DrawText( img, ss.str(), gravity, 5, 0, sign * (10 + rect_i.height + rect_a.height)); diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 5c0bd5c..e838e6c 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -30,23 +30,25 @@ MYNTEYE_BEGIN_NAMESPACE -mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1) { +mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1, + const int &accel_range, + const int &gyro_range) { 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.accel[0] = pak1.segments[i].accel[0] * 1.f * accel_range / 0x10000; + tpr.accel[1] = pak1.segments[i].accel[1] * 1.f * accel_range / 0x10000; + tpr.accel[2] = pak1.segments[i].accel[2] * 1.f * accel_range / 0x10000; + tpr.gyro[0] = pak1.segments[i].gyro[0] * gyro_range / 0x10000; + tpr.gyro[1] = pak1.segments[i].gyro[1] * gyro_range / 0x10000; + tpr.gyro[2] = pak1.segments[i].gyro[2] * gyro_range / 0x10000; 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.temperature = pak1.segments[i].temperature / 326.8f + 25; tpr.timestamp = pak1.segments[i].timestamp; res.segments.push_back(tpr); } @@ -142,6 +144,13 @@ Channels::Channels(const std::shared_ptr &device, dev_info_(nullptr) { VLOG(2) << __func__; UpdateControlInfos(); + accel_range = GetControlValue(Option::ACCELEROMETER_RANGE); + if (accel_range == -1) + accel_range = GetAccelRangeDefault(); + + gyro_range = GetControlValue(Option::GYROSCOPE_RANGE); + if (gyro_range == -1) + gyro_range = GetGyroRangeDefault(); } Channels::~Channels() { @@ -448,7 +457,7 @@ void Channels::DoImuTrack1() { if (imu_callback_) { for (auto &&packet : res_packet.packets) { - imu_callback_(to_pak2(packet)); + imu_callback_(to_pak2(packet, accel_range, gyro_range)); } } @@ -459,23 +468,18 @@ void Channels::DoImuTrack2() { // LOG(INFO) << "wait to adapter!"; 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) { @@ -483,16 +487,23 @@ LOG(INFO) << 5; } 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) { + // LOG(INFO) << "packet count:" << (int)packet.count; + // for (size_t i = 0; i < (int)packet.count; i++) { // NOLINT + // LOG(INFO) << "accel:" << packet.segments[i].accel[0]; + // LOG(INFO) << packet.segments[i].accel[1]; + // LOG(INFO) << packet.segments[i].accel[2]; + // LOG(INFO) << "gyro:" << packet.segments[i].gyro[0]; + // LOG(INFO) << packet.segments[i].gyro[1]; + // LOG(INFO) << packet.segments[i].gyro[2]; + // } imu_callback_(packet); } } @@ -888,13 +899,11 @@ bool Channels::XuImuRead(ImuResPacket2 *res) const { 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)) { adapter_->GetImuResPacket(data, res); - if (res->header != 0x5B) { LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x" << std::hex << std::uppercase << std::setw(2) diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h index 7e623fd..b8319ce 100644 --- a/src/mynteye/device/channel/channels.h +++ b/src/mynteye/device/channel/channels.h @@ -142,6 +142,8 @@ class MYNTEYE_API Channels { bool enable_imu_correspondence; std::thread imu_track_thread_; volatile bool imu_track_stop_; + int accel_range; + int gyro_range; std::uint32_t imu_sn_; imu_callback_t imu_callback_; diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h index ee10bd9..4138ab0 100644 --- a/src/mynteye/device/channel/def.h +++ b/src/mynteye/device/channel/def.h @@ -119,9 +119,9 @@ struct ImuSegment2 { 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]; + float temperature; + float accel[3]; + float gyro[3]; }; #pragma pack(pop) diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index e48d6fc..e901694 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -85,13 +85,34 @@ void Motions::SetMotionCallback(motion_callback_t callback) { imu->timestamp = seg.timestamp; imu->flag = seg.flag; imu->is_ets = seg.is_ets; - imu->temperature = seg.temperature / 326.8f + 25; - 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; + // imu->temperature = seg.temperature / 326.8f + 25; + imu->temperature = seg.temperature; + // LOG(INFO) << "beforea" << seg.accel[0]; + // LOG(INFO) << "before" << seg.accel[1]; + // LOG(INFO) << "before" << seg.accel[2]; + // LOG(INFO) << "beforeg" << seg.gyro[0]; + // LOG(INFO) << "before" << seg.gyro[1]; + // LOG(INFO) << "before" << seg.gyro[2]; + // 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; + + imu->accel[0] = seg.accel[0]; + imu->accel[1] = seg.accel[1]; + imu->accel[2] = seg.accel[2]; + imu->gyro[0] = seg.gyro[0]; + imu->gyro[1] = seg.gyro[1]; + imu->gyro[2] = seg.gyro[2]; + + // LOG(INFO)<< "aftera" << imu->accel[0]; + // LOG(INFO)<< "after" << imu->accel[1]; + // LOG(INFO)<< "after" << imu->accel[2]; + // LOG(INFO)<< "afterg" << imu->gyro[0]; + // LOG(INFO)<< "afterg" << imu->gyro[1]; + // LOG(INFO)<< "afterg" << imu->gyro[2]; bool proc_assembly = ((proc_mode_ & ProcessMode::PROC_IMU_ASSEMBLY) > 0); bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0); diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc index 8083b88..39da2f3 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.cc +++ b/src/mynteye/device/standard2/channels_adapter_s2.cc @@ -76,25 +76,17 @@ struct ImuData2 { } 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); + frame_id = BYTE_4(data, 0); + timestamp = BYTE_8((u_char*)data, 4); // NOLINT flag = *(data + 12); - temperature = *((float*)(data+ 13)); - LOG(INFO) << "temperature:" << temperature; - accel_or_gyro[0] = *((float*)(data + 17)); - LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0]; - accel_or_gyro[1] = *((float*)(data + 21)); - LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1]; - accel_or_gyro[2] = *((float*)(data + 25)); - LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2]; + temperature = *((float*)(data+ 13)); // NOLINT + // LOG(INFO) << "temperature:" << temperature; + accel_or_gyro[0] = *((float*)(data + 17)); // NOLINT + // LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0]; + accel_or_gyro[1] = *((float*)(data + 21)); // NOLINT + // LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1]; + accel_or_gyro[2] = *((float*)(data + 25)); // NOLINT + // LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2]; } }; @@ -115,18 +107,36 @@ void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) { } 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; + if (seg->flag == 1) { + // LOG(INFO) << "flag1"; + seg->accel[0] = imu.accel_or_gyro[0]; + seg->accel[1] = imu.accel_or_gyro[1]; + seg->accel[2] = imu.accel_or_gyro[2]; + seg->gyro[0] = 0.; + seg->gyro[1] = 0.; + seg->gyro[2] = 0.; + } else if (seg->flag == 2) { + // LOG(INFO) << "flag2"; + seg->gyro[0] = imu.accel_or_gyro[0]; + seg->gyro[1] = imu.accel_or_gyro[1]; + seg->gyro[2] = imu.accel_or_gyro[2]; + seg->accel[0] = 0.; + seg->accel[1] = 0.; + seg->accel[2] = 0.; + } else if (seg->flag == 3) { + LOG(INFO) << "flag3"; + // seg->gyro[0] = imu.accel_or_gyro[0]; + // seg->gyro[1] = imu.accel_or_gyro[1]; + // seg->gyro[2] = imu.accel_or_gyro[2]; + // seg->accel[0] = 0.; + // seg->accel[1] = 0.; + // seg->accel[2] = 0.; + } } void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) { @@ -181,7 +191,6 @@ void unpack_imu_res_packet2(const std::uint8_t *data, ImuResPacket2 *res) { // 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);