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

Merge branch 'imu_2.0_protocl' into develop

Browse Source
This commit is contained in:
TinyO 2019-10-09 11:16:21 +08:00
commit a499136449
14 changed files with 402 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/mynteye/api/api.h
View File

@ -347,7 +347,12 @@ class MYNTEYE_API API {
std::vector<api::MotionData> GetMotionDatas();
/**
* Enable motion datas with timestamp correspondence of some stream.
* enable motion datas timestamp correspondence in device.
*/
void EnableImuTimestampCorrespondence(bool is_enable);
/**
* Enable motion datas with timestamp correspondence of some stream in api.
*/
void EnableTimestampCorrespondence(const Stream &stream,
bool keep_accel_then_gyro = true);
@ -385,6 +390,9 @@ class MYNTEYE_API API {
motion_callback_t callback_;
bool api_correspondence_enable_;
bool dev_correspondence_enable_;
void CheckImageParams();
};

4
include/mynteye/device/device.h
View File

@ -290,6 +290,10 @@ class MYNTEYE_API Device {
* Enable cache motion datas.
*/
void EnableMotionDatas();
/**
* Enable motion datas timestamp correspondence.
*/
void EnableImuCorrespondence(bool is_enable);
/**
* Enable cache motion datas.
*/

2
samples/get_imu.cc
View File

@ -31,6 +31,8 @@ int main(int argc, char *argv[]) {
// Enable this will cache the motion datas until you get them.
api->EnableMotionDatas();
// Enable imu timestamp correspondence int device;
api->EnableImuTimestampCorrespondence(true);
api->Start(Source::ALL);

19
samples/util_cv.cc
View File

@ -167,23 +167,8 @@ cv::Rect CVPainter::DrawImuData(
img, ss.str(), gravity, 5, 0,
sign * (10 + rect_i.height + rect_a.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 * (15 + rect_i.height + rect_a.height + rect_p.height));

26
src/mynteye/api/api.cc
View File

@ -213,7 +213,9 @@ std::vector<std::string> get_plugin_paths() {
} // namespace
API::API(std::shared_ptr<Device> device, CalibrationModel calib_model)
: device_(device), correspondence_(nullptr) {
: device_(device), correspondence_(nullptr),
api_correspondence_enable_(false),
dev_correspondence_enable_(false) {
VLOG(2) << __func__;
// std::dynamic_pointer_cast<StandardDevice>(device_);
synthetic_.reset(new Synthetic(this, calib_model));
@ -505,6 +507,15 @@ std::vector<api::MotionData> API::GetMotionDatas() {
void API::EnableTimestampCorrespondence(const Stream &stream,
bool keep_accel_then_gyro) {
if (!dev_correspondence_enable_) {
api_correspondence_enable_ = keep_accel_then_gyro;
} else {
LOG(WARNING) << "dev_correspondence_enable_ "
"has been set to true, "
"you should close it first when you want to use "
"api_correspondence_enable_.";
return;
}
if (correspondence_ == nullptr) {
correspondence_.reset(new Correspondence(device_, stream));
correspondence_->KeepAccelThenGyro(keep_accel_then_gyro);
@ -526,6 +537,19 @@ void API::EnableTimestampCorrespondence(const Stream &stream,
}
}
void API::EnableImuTimestampCorrespondence(bool is_enable) {
if (!api_correspondence_enable_) {
dev_correspondence_enable_= is_enable;
} else {
LOG(WARNING) << "api_correspondence_enable_ "
"has been set to true, "
"you should close it first when you want to use "
"dev_correspondence_enable_.";
return;
}
device_->EnableImuCorrespondence(is_enable);
}
void API::EnablePlugin(const std::string &path) {
static DL dl;
CHECK(dl.Open(path.c_str())) << "Open plugin failed: " << path;

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

@ -30,6 +30,32 @@
MYNTEYE_BEGIN_NAMESPACE
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] * 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 / 326.8f + 25;
tpr.timestamp = pak1.segments[i].timestamp;
res.segments.push_back(tpr);
}
return res;
}
namespace {
const uvc::xu mynteye_xu = {3, 2,
@ -112,12 +138,20 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
adapter_(adapter),
is_imu_tracking_(false),
is_imu_proto2_(false),
enable_imu_correspondence(false),
imu_track_stop_(false),
imu_sn_(0),
imu_callback_(nullptr),
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() {
@ -328,7 +362,7 @@ bool Channels::SetControlValue(const Option &option, std::uint64_t value) {
case Option::MIN_EXPOSURE_TIME:
case Option::IIC_ADDRESS_SETTING:
case Option::ZERO_DRIFT_CALIBRATION:
LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)";
LOG(WARNING) << option << " refer to function SetControlValue(const Option &option, std::int32_t value)"; // NOLINT
break;
case Option::ERASE_CHIP:
LOG(WARNING) << option << " set value useless";
@ -379,7 +413,7 @@ void Channels::SetImuCallback(imu_callback_t callback) {
}
void Channels::DoImuTrack() {
if (IsImuProc2()) {
if (IsImuProtocol2()) {
return DoImuTrack2();
} else {
return DoImuTrack1();
@ -424,7 +458,7 @@ void Channels::DoImuTrack1() {
if (imu_callback_) {
for (auto &&packet : res_packet.packets) {
imu_callback_(packet);
imu_callback_(to_pak2(packet, accel_range, gyro_range));
}
}
@ -432,49 +466,41 @@ void Channels::DoImuTrack1() {
}
void Channels::DoImuTrack2() {
// static ImuReqPacket req_packet{0};
// static ImuResPacket res_packet;
// LOG(INFO) << "wait to adapter!";
static ImuReqPacket2 req_packet{0x5A, imu_sn_, enable_imu_correspondence};
static ImuResPacket2 res_packet;
if (!XuImuWrite(req_packet)) {
return;
}
if (!XuImuRead(&res_packet)) {
return;
}
if (res_packet.packets.size() == 0) {
return;
}
if (res_packet.packets.back().count == 0) {
return;
}
VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
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";
return;
}
imu_sn_ = sn;
if (imu_callback_) {
for (auto &&packet : res_packet.packets) {
imu_callback_(packet);
}
}
// req_packet.serial_number = imu_sn_;
// if (!XuImuWrite(req_packet)) {
// return;
// }
// if (!XuImuRead(&res_packet)) {
// return;
// }
// if (res_packet.packets.size() == 0) {
// return;
// }
// if (res_packet.packets.back().count == 0) {
// return;
// }
// VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
// 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";
// return;
// }
// imu_sn_ = sn;
// if (imu_callback_) {
// for (auto &&packet : res_packet.packets) {
// imu_callback_(packet);
// }
// }
// res_packet.packets.clear();
LOG(INFO) << "wait to adapter!";
res_packet.packets.clear();
}
@ -499,7 +525,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
<< ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
}
};
if (IsImuProc2()) {
if (IsImuProtocol2()) {
while (!imu_track_stop_) {
auto &&time_beg = times::now();
DoImuTrack2();
@ -801,7 +827,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
static_cast<std::uint8_t>((value >> 56) & 0xFF)};
if (XuControlQuery(CHANNEL_HALF_DUPLEX, uvc::XU_QUERY_SET, 20, data)) {
VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value
VLOG(2) << "XuHalfDuplexSet value (0x" << std::hex << std::uppercase << value // NOLINT
<< ") of " << option << " success";
return true;
} else {
@ -813,7 +839,7 @@ bool Channels::XuHalfDuplexSet(Option option, std::uint64_t value) const {
bool Channels::XuImuWrite(const ImuReqPacket &req) const {
auto &&data = req.to_data();
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4];
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4]; // NOLINT
if (XuControlQuery(
CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
VLOG(2) << "XuImuWrite request success";
@ -824,12 +850,48 @@ bool Channels::XuImuWrite(const ImuReqPacket &req) const {
}
}
bool Channels::XuImuWrite(const ImuReqPacket2 &req) const {
auto &&data = req.to_data();
// LOG(INFO) << data.size() << "||" << (int)data[0] << " " << (int)data[1] << " " << (int)data[2] << " " << (int)data[3] << " " << (int)data[4]; // NOLINT
if (XuControlQuery(
CHANNEL_IMU_WRITE, uvc::XU_QUERY_SET, data.size(), data.data())) {
VLOG(2) << "XuImuWrite request success";
return true;
} else {
LOG(WARNING) << "XuImuWrite request failed";
return false;
}
}
bool Channels::XuImuRead(ImuResPacket2 *res) const {
static std::uint8_t data[2000]{};
if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
adapter_->GetImuResPacket2(data, res, enable_imu_correspondence);
if (res->header != 0x5B) {
LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
<< std::hex << std::uppercase << std::setw(2)
<< std::setfill('0') << static_cast<int>(res->header)
<< " now";
return false;
}
if (res->state != 0) {
LOG(WARNING) << "Imu response packet state must be 0, but " << res->state // NOLINT
<< " now";
return false;
}
VLOG(2) << "XuImuRead response success";
return true;
} else {
LOG(WARNING) << "XuImuRead response failed";
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)

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

@ -54,7 +54,7 @@ class MYNTEYE_API Channels {
XU_CMD_LAST
} xu_cmd_t;
using imu_callback_t = std::function<void(const ImuPacket &packet)>;
using imu_callback_t = std::function<void(const ImuPacket2 &packet)>;
using device_info_t = FileChannel::device_info_t;
using img_params_t = FileChannel::img_params_t;
@ -89,7 +89,10 @@ class MYNTEYE_API Channels {
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
bool SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
inline bool IsImuProc2() const { return is_imu_proto2_; }
inline bool IsImuProtocol2() const { return is_imu_proto2_; }
inline void EnableImuCorrespondence(bool is_enable) {
enable_imu_correspondence = is_enable;
}
private:
bool PuControlRange(
@ -118,7 +121,9 @@ class MYNTEYE_API Channels {
bool XuHalfDuplexSet(Option option, std::uint64_t value) const;
bool XuImuWrite(const ImuReqPacket &req) const;
bool XuImuWrite(const ImuReqPacket2 &req) const;
bool XuImuRead(ImuResPacket *res) const;
bool XuImuRead(ImuResPacket2 *res) const;
bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
@ -134,8 +139,11 @@ class MYNTEYE_API Channels {
bool is_imu_tracking_;
bool is_imu_proto2_;
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_;
@ -157,6 +165,8 @@ class ChannelsAdapter {
virtual std::vector<std::int32_t> GetGyroRangeValues() = 0;
virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
virtual void GetImuResPacket2(const std::uint8_t *data,
ImuResPacket2 *res, bool is_correspondence_on) = 0;
protected:
Model model_;

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

@ -63,6 +63,34 @@ struct ImuReqPacket {
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu request packet 2.0.
*/
#pragma pack(push, 1)
struct ImuReqPacket2 {
std::uint8_t header;
std::uint32_t serial_number;
std::uint8_t correspondence_switch;
ImuReqPacket2() = default;
explicit ImuReqPacket2(std::uint32_t serial_number)
: ImuReqPacket2(0x5A, serial_number) {}
ImuReqPacket2(std::uint8_t header, std::uint32_t serial_number)
: ImuReqPacket2(header, serial_number, false) {}
ImuReqPacket2(std::uint8_t header,
std::uint32_t serial_number,
bool correspondence_switch_in)
: header(header),
serial_number(serial_number),
correspondence_switch(correspondence_switch_in ? 1:0) {}
std::array<std::uint8_t, 5> to_data() const {
return {header, correspondence_switch, 0, 0, 0};
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu segment.
@ -80,6 +108,23 @@ struct ImuSegment {
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu segment.
*/
#pragma pack(push, 1)
struct ImuSegment2 {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
// Is external time source
bool is_ets;
float temperature;
float accel[3];
float gyro[3];
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu packet.
@ -92,6 +137,18 @@ struct ImuPacket {
std::vector<ImuSegment> segments;
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu packet.
*/
#pragma pack(push, 1)
struct ImuPacket2 {
std::uint8_t version;
std::uint8_t count;
std::uint32_t serial_number;
std::vector<ImuSegment2> segments;
};
#pragma pack(pop)
/**
* @ingroup datatypes
@ -108,6 +165,21 @@ struct ImuResPacket {
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu response packet.
*/
#pragma pack(push, 1)
struct ImuResPacket2 {
std::uint8_t version;
std::uint8_t header;
std::uint8_t state;
std::uint16_t size;
std::vector<ImuPacket2> packets;
std::uint8_t checksum;
};
#pragma pack(pop)
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_CHANNEL_DEF_H_

4
src/mynteye/device/device.cc
View File

@ -602,6 +602,10 @@ void Device::StopMotionTracking() {
void Device::OnStereoStreamUpdate() {}
void Device::EnableImuCorrespondence(bool is_enable) {
channels_->EnableImuCorrespondence(is_enable);
}
void Device::ReadAllInfos() {
device_info_ = std::make_shared<DeviceInfo>();

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

@ -70,7 +70,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
if (gyro_range == -1)
gyro_range = channels_->GetGyroRangeDefault();
channels_->SetImuCallback([this](const ImuPacket &packet) {
channels_->SetImuCallback([this](const ImuPacket2 &packet) {
if (!motion_callback_ && !motion_datas_enabled_) {
return;
}
@ -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);

5
src/mynteye/device/standard/channels_adapter_s.cc
View File

@ -122,4 +122,9 @@ void StandardChannelsAdapter::GetImuResPacket(
unpack_imu_res_packet(data, res);
}
void StandardChannelsAdapter::GetImuResPacket2(
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
}
MYNTEYE_END_NAMESPACE

4
src/mynteye/device/standard/channels_adapter_s.h
View File

@ -35,6 +35,10 @@ class StandardChannelsAdapter : public ChannelsAdapter {
std::vector<std::int32_t> GetGyroRangeValues() override;
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
void GetImuResPacket2(
const std::uint8_t *data,
ImuResPacket2 *res,
bool is_correspondence_on) override;
};
MYNTEYE_END_NAMESPACE

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

@ -52,6 +52,53 @@ struct ImuData {
};
#pragma pack(pop)
#pragma pack(push, 1)
#define BYTE_4(data, begin) (*(data + begin) << 24) | \
(*(data + begin + 1) << 16) | \
(*(data + begin + 2) << 8) | \
*(data + begin + 3)
struct ImuData2 {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
float temperature;
float accel_or_gyro[3];
float gyro_add[3];
ImuData2() = default;
explicit ImuData2(const std::uint8_t *data) {
from_data(data);
}
void from_data(const std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
frame_id = BYTE_4(data, 0);
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 = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
flag = *(data + 12);
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];
if (flag == 3) {
gyro_add[0] = *((float*)(data + 29)); // NOLINT
gyro_add[1] = *((float*)(data + 33)); // NOLINT
gyro_add[2] = *((float*)(data + 37)); // NOLINT
}
}
};
#pragma pack(pop)
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp;
@ -66,6 +113,36 @@ void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
}
void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
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;
if (seg->flag == 1) {
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) {
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) {
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] = imu.gyro_add[0];
seg->accel[1] = imu.gyro_add[1];
seg->accel[2] = imu.gyro_add[2];
}
}
void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
std::size_t data_n = sizeof(ImuData); // 21
for (std::size_t i = 0; i < pkg->count; i++) {
@ -81,6 +158,25 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
}
}
void unpack_imu_packet2(
const std::uint8_t *data, ImuPacket2 *pkg, bool is_correspondence_on) {
std::size_t data_n = 29;
if (is_correspondence_on) {
data_n = 41;
}
for (std::size_t i = 0; i < pkg->count; i++) {
ImuSegment2 seg;
unpack_imu_segment2(ImuData2(data + data_n * i), &seg);
pkg->segments.push_back(seg);
}
if (pkg->count) {
pkg->serial_number = pkg->segments.back().frame_id;
} else {
LOG(ERROR) << "The imu data pipeline lost more than 5 samples continuously, "
<< "please check the device and firmware";
}
}
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
res->header = *data;
res->state = *(data + 1);
@ -94,6 +190,24 @@ void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
res->checksum = *(data + 4 + res->size);
}
void unpack_imu_res_packet2(
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
res->header = *data;
// u_int64_t* jj = (u_int64_t*) data;
res->state = *(data + 1);
res->size = (*(data + 2) << 8) | *(data + 3);
std::size_t data_n = 29;
if (is_correspondence_on) {
data_n = 41;
}
// LOG(INFO) << "size:" << data_n;
ImuPacket2 packet;
packet.count = res->size / data_n;
unpack_imu_packet2(data + 4, &packet, is_correspondence_on);
res->packets.push_back(packet);
res->checksum = *(data + 4 + res->size);
}
} // namespace
Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
@ -124,4 +238,9 @@ void Standard2ChannelsAdapter::GetImuResPacket(
unpack_imu_res_packet(data, res);
}
void Standard2ChannelsAdapter::GetImuResPacket2(
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
unpack_imu_res_packet2(data, res, is_correspondence_on);
}
MYNTEYE_END_NAMESPACE

4
src/mynteye/device/standard2/channels_adapter_s2.h
View File

@ -35,6 +35,10 @@ class Standard2ChannelsAdapter : public ChannelsAdapter {
std::vector<std::int32_t> GetGyroRangeValues() override;
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
void GetImuResPacket2(
const std::uint8_t *data,
ImuResPacket2 *res,
bool is_correspondence_on) override;
};
MYNTEYE_END_NAMESPACE