tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(imu): imu2.0 first complete.

Browse Source
This commit is contained in:
TinyO 2019-09-29 15:11:16 +08:00
parent 997b6102d4
commit 7e0e4b83e7
6 changed files with 97 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
samples/util_cv.cc
View File

@ -141,23 +141,8 @@ cv::Rect CVPainter::DrawImuData(
cv::Rect rect_a = cv::Rect rect_a =
DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height)); DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
static double gyro1_s = 0.0; Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
static double gyro2_s = 0.0; << data.gyro[1] << "," << fmt_imu << data.gyro[2];
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;
cv::Rect rect_g = DrawText( cv::Rect rect_g = DrawText(
img, ss.str(), gravity, 5, 0, img, ss.str(), gravity, 5, 0,
sign * (10 + rect_i.height + rect_a.height)); sign * (10 + rect_i.height + rect_a.height));

45
src/mynteye/device/channel/channels.cc
View File

@ -30,23 +30,25 @@
MYNTEYE_BEGIN_NAMESPACE 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; mynteye::ImuPacket2 res;
res.version = pak1.version; res.version = pak1.version;
res.count = pak1.count; res.count = pak1.count;
res.serial_number = pak1.serial_number; res.serial_number = pak1.serial_number;
for (size_t i = 0; i < pak1.segments.size(); i++) { for (size_t i = 0; i < pak1.segments.size(); i++) {
mynteye::ImuSegment2 tpr; mynteye::ImuSegment2 tpr;
tpr.accel[0] = pak1.segments[i].accel[0]; tpr.accel[0] = pak1.segments[i].accel[0] * 1.f * accel_range / 0x10000;
tpr.accel[1] = pak1.segments[i].accel[1]; tpr.accel[1] = pak1.segments[i].accel[1] * 1.f * accel_range / 0x10000;
tpr.accel[2] = pak1.segments[i].accel[2]; tpr.accel[2] = pak1.segments[i].accel[2] * 1.f * accel_range / 0x10000;
tpr.gyro[0] = pak1.segments[i].gyro[0]; tpr.gyro[0] = pak1.segments[i].gyro[0] * gyro_range / 0x10000;
tpr.gyro[1] = pak1.segments[i].gyro[1]; tpr.gyro[1] = pak1.segments[i].gyro[1] * gyro_range / 0x10000;
tpr.gyro[2] = pak1.segments[i].gyro[2]; tpr.gyro[2] = pak1.segments[i].gyro[2] * gyro_range / 0x10000;
tpr.flag = pak1.segments[i].flag; tpr.flag = pak1.segments[i].flag;
tpr.frame_id = pak1.segments[i].frame_id; tpr.frame_id = pak1.segments[i].frame_id;
tpr.is_ets = pak1.segments[i].is_ets; 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; tpr.timestamp = pak1.segments[i].timestamp;
res.segments.push_back(tpr); res.segments.push_back(tpr);
} }
@ -142,6 +144,13 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
dev_info_(nullptr) { dev_info_(nullptr) {
VLOG(2) << __func__; VLOG(2) << __func__;
UpdateControlInfos(); 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() { Channels::~Channels() {
@ -448,7 +457,7 @@ void Channels::DoImuTrack1() {
if (imu_callback_) { if (imu_callback_) {
for (auto &&packet : res_packet.packets) { 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!"; // LOG(INFO) << "wait to adapter!";
static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence}; static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
static ImuResPacket2 res_packet; static ImuResPacket2 res_packet;
LOG(INFO) << 1;
if (!XuImuWrite(req_packet)) { if (!XuImuWrite(req_packet)) {
return; return;
} }
LOG(INFO) << 2;
if (!XuImuRead(&res_packet)) { if (!XuImuRead(&res_packet)) {
return; return;
} }
LOG(INFO) << 3;
if (res_packet.packets.size() == 0) { if (res_packet.packets.size() == 0) {
return; return;
} }
LOG(INFO) << 4;
if (res_packet.packets.back().count == 0) { if (res_packet.packets.back().count == 0) {
return; return;
} }
LOG(INFO) << 5;
VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() { VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
std::size_t n = 0; std::size_t n = 0;
for (auto &&packet : res_packet.packets) { for (auto &&packet : res_packet.packets) {
@ -483,16 +487,23 @@ LOG(INFO) << 5;
} }
return n; return n;
}(); }();
LOG(INFO) << 6;
auto &&sn = res_packet.packets.back().serial_number; auto &&sn = res_packet.packets.back().serial_number;
if (imu_sn_ == sn) { if (imu_sn_ == sn) {
VLOG(2) << "New imu not ready, dropped"; VLOG(2) << "New imu not ready, dropped";
return; return;
} }
imu_sn_ = sn; imu_sn_ = sn;
LOG(INFO) << 7;
if (imu_callback_) { if (imu_callback_) {
for (auto &&packet : res_packet.packets) { 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); imu_callback_(packet);
} }
} }
@ -888,13 +899,11 @@ bool Channels::XuImuRead(ImuResPacket2 *res) const {
return false; return false;
} }
} }
bool Channels::XuImuRead(ImuResPacket *res) const { bool Channels::XuImuRead(ImuResPacket *res) const {
static std::uint8_t data[2000]{}; static std::uint8_t data[2000]{};
// std::fill(data, data + 2000, 0); // reset // std::fill(data, data + 2000, 0); // reset
if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) { if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
adapter_->GetImuResPacket(data, res); adapter_->GetImuResPacket(data, res);
if (res->header != 0x5B) { if (res->header != 0x5B) {
LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x" LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
<< std::hex << std::uppercase << std::setw(2) << std::hex << std::uppercase << std::setw(2)

2
src/mynteye/device/channel/channels.h
View File

@ -142,6 +142,8 @@ class MYNTEYE_API Channels {
bool enable_imu_correspondence; bool enable_imu_correspondence;
std::thread imu_track_thread_; std::thread imu_track_thread_;
volatile bool imu_track_stop_; volatile bool imu_track_stop_;
int accel_range;
int gyro_range;
std::uint32_t imu_sn_; std::uint32_t imu_sn_;
imu_callback_t imu_callback_; imu_callback_t imu_callback_;

6
src/mynteye/device/channel/def.h
View File

@ -119,9 +119,9 @@ struct ImuSegment2 {
std::uint8_t flag; std::uint8_t flag;
// Is external time source // Is external time source
bool is_ets; bool is_ets;
std::int32_t temperature; float temperature;
std::int32_t accel[3]; float accel[3];
std::int32_t gyro[3]; float gyro[3];
}; };
#pragma pack(pop) #pragma pack(pop)

35
src/mynteye/device/motions.cc
View File

@ -85,13 +85,34 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
imu->timestamp = seg.timestamp; imu->timestamp = seg.timestamp;
imu->flag = seg.flag; imu->flag = seg.flag;
imu->is_ets = seg.is_ets; imu->is_ets = seg.is_ets;
imu->temperature = seg.temperature / 326.8f + 25; // imu->temperature = seg.temperature / 326.8f + 25;
imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000; imu->temperature = seg.temperature;
imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000; // LOG(INFO) << "beforea" << seg.accel[0];
imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000; // LOG(INFO) << "before" << seg.accel[1];
imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000; // LOG(INFO) << "before" << seg.accel[2];
imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000; // LOG(INFO) << "beforeg" << seg.gyro[0];
imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000; // 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_assembly = ((proc_mode_ & ProcessMode::PROC_IMU_ASSEMBLY) > 0);
bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0); bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0);

61
src/mynteye/device/standard2/channels_adapter_s2.cc
View File

@ -76,25 +76,17 @@ struct ImuData2 {
} }
void from_data(const std::uint8_t *data) { void from_data(const std::uint8_t *data) {
std::uint32_t timestamp_l; frame_id = BYTE_4(data, 0);
std::uint32_t timestamp_h; timestamp = BYTE_8((u_char*)data, 4); // NOLINT
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);
flag = *(data + 12); flag = *(data + 12);
temperature = *((float*)(data+ 13)); temperature = *((float*)(data+ 13)); // NOLINT
LOG(INFO) << "temperature:" << temperature; // LOG(INFO) << "temperature:" << temperature;
accel_or_gyro[0] = *((float*)(data + 17)); accel_or_gyro[0] = *((float*)(data + 17)); // NOLINT
LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0]; // LOG(INFO) << "accel_or_gyro[0]:" << accel_or_gyro[0];
accel_or_gyro[1] = *((float*)(data + 21)); accel_or_gyro[1] = *((float*)(data + 21)); // NOLINT
LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1]; // LOG(INFO) << "accel_or_gyro[1]:" << accel_or_gyro[1];
accel_or_gyro[2] = *((float*)(data + 25)); accel_or_gyro[2] = *((float*)(data + 25)); // NOLINT
LOG(INFO) << "accel_or_gyro[2]:" << accel_or_gyro[2]; // 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) { void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
LOG(INFO) << "unpack_imu_segment2" << imu.timestamp;
seg->frame_id = imu.frame_id; seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp; seg->timestamp = imu.timestamp;
seg->flag = imu.flag & 0b0011; seg->flag = imu.flag & 0b0011;
seg->is_ets = ((imu.flag & 0b0100) == 0b0100); seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
seg->temperature = imu.temperature; seg->temperature = imu.temperature;
seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0; if (seg->flag == 1) {
seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0; // LOG(INFO) << "flag1";
seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0; seg->accel[0] = imu.accel_or_gyro[0];
seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0; seg->accel[1] = imu.accel_or_gyro[1];
seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0; seg->accel[2] = imu.accel_or_gyro[2];
seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0; 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) { 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; // LOG(INFO) << "size:" << data_n;
ImuPacket2 packet; ImuPacket2 packet;
packet.count = res->size / data_n; packet.count = res->size / data_n;
LOG(INFO) << "count:" << (int)(packet.count);
unpack_imu_packet2(data + 4, &packet); unpack_imu_packet2(data + 4, &packet);
res->packets.push_back(packet); res->packets.push_back(packet);
res->checksum = *(data + 4 + res->size); res->checksum = *(data + 4 + res->size);