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

feat(imu): imu 2.0 protocl.

Browse Source
This commit is contained in:
TinyO 2019-09-27 18:15:03 +08:00
parent db4f1908ed
commit 817057875b
8 changed files with 297 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,6 +30,30 @@
MYNTEYE_BEGIN_NAMESPACE
mynteye::ImuPacket2 to_pak2(const mynteye::ImuPacket& pak1) {
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.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.timestamp = pak1.segments[i].timestamp;
res.segments.push_back(tpr);
}
return res;
}
namespace {
const uvc::xu mynteye_xu = {3, 2,
@ -328,7 +352,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 +403,7 @@ void Channels::SetImuCallback(imu_callback_t callback) {
}
void Channels::DoImuTrack() {
if (IsImuProc2()) {
if (IsImuProtocol2()) {
return DoImuTrack2();
} else {
return DoImuTrack1();
@ -424,7 +448,7 @@ void Channels::DoImuTrack1() {
if (imu_callback_) {
for (auto &&packet : res_packet.packets) {
imu_callback_(packet);
imu_callback_(to_pak2(packet));
}
}
@ -432,49 +456,48 @@ 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;
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) {
n += packet.count;
}
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) {
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 +522,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 +824,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 +836,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,6 +847,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);
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;
}
// LOG(INFO) << res->size;
if (enable_imu_correspondence) {
LOG(INFO) << "enable_imu_correspondence";
} else {
}
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

12
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,6 +139,7 @@ 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_;
@ -157,6 +163,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) = 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;
std::int32_t temperature;
std::int32_t accel[3];
std::int32_t 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_

2
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;
}

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) {
LOG(WARNING) << "s1 device can't use ImuResPacket2.0 check the firmware.";
}
MYNTEYE_END_NAMESPACE

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

@ -35,6 +35,7 @@ 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) override;
};
MYNTEYE_END_NAMESPACE

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

@ -52,6 +52,50 @@ struct ImuData {
};
#pragma pack(pop)
#pragma pack(push, 1)
#define BYTE_8(data, begin) (static_cast<std::uint64_t>( \
((*(data + begin) << 24) | (*(data + begin+1) << 16) | \
(*(data + begin+2) << 8) | *(data + begin+3)) << 32) | \
((*(data + begin+4) << 24) | (*(data + begin+5) << 16) | \
(*(data + begin+6) << 8) | *(data + begin+7)))
#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];
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 = (*(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);
temperature = BYTE_4(data, 13);
// accel_or_gyro[0] = BYTE_4(data, 17);
// accel_or_gyro[1] = BYTE_4(data, 21);
// accel_or_gyro[2] = BYTE_4(data, 25);
}
};
#pragma pack(pop)
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp;
@ -66,6 +110,21 @@ 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) {
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;
}
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 +140,21 @@ void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
}
}
void unpack_imu_packet2(const std::uint8_t *data, ImuPacket2 *pkg) {
std::size_t data_n = sizeof(ImuData2); // 29
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 +168,21 @@ 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) {
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 = sizeof(ImuData2); // 21
// 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);
}
} // namespace
Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
@ -124,4 +213,9 @@ void Standard2ChannelsAdapter::GetImuResPacket(
unpack_imu_res_packet(data, res);
}
void Standard2ChannelsAdapter::GetImuResPacket2(
const std::uint8_t *data, ImuResPacket2 *res) {
unpack_imu_res_packet2(data, res);
}
MYNTEYE_END_NAMESPACE

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

@ -35,6 +35,7 @@ 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) override;
};
MYNTEYE_END_NAMESPACE