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

feat(imu): imu2.0 decond part complete.

Browse Source
This commit is contained in:
TinyO 2019-09-29 16:42:44 +08:00
parent 7e0e4b83e7
commit 283f0b141c
11 changed files with 90 additions and 37 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);

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;

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

@ -138,6 +138,7 @@ 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),
@ -495,15 +496,6 @@ void Channels::DoImuTrack2() {
imu_sn_ = sn;
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);
}
}
@ -874,7 +866,7 @@ bool Channels::XuImuWrite(const ImuReqPacket2 &req) const {
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);
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)

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

@ -166,7 +166,7 @@ class ChannelsAdapter {
virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
virtual void GetImuResPacket2(const std::uint8_t *data,
ImuResPacket2 *res) = 0;
ImuResPacket2 *res, bool is_correspondence_on) = 0;
protected:
Model model_;

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>();

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

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

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

@ -35,7 +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) override;
void GetImuResPacket2(
const std::uint8_t *data,
ImuResPacket2 *res,
bool is_correspondence_on) override;
};
MYNTEYE_END_NAMESPACE

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

@ -54,11 +54,6 @@ struct ImuData {
#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) | \
@ -69,6 +64,7 @@ struct ImuData2 {
std::uint8_t flag;
float temperature;
float accel_or_gyro[3];
float gyro_add[3];
ImuData2() = default;
explicit ImuData2(const std::uint8_t *data) {
@ -76,8 +72,14 @@ struct ImuData2 {
}
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 = BYTE_8((u_char*)data, 4); // NOLINT
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;
@ -87,6 +89,11 @@ struct ImuData2 {
// 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
}
}
};
@ -113,7 +120,6 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
seg->is_ets = ((imu.flag & 0b0100) == 0b0100);
seg->temperature = imu.temperature;
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];
@ -121,7 +127,6 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
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];
@ -130,12 +135,12 @@ void unpack_imu_segment2(const ImuData2 &imu, ImuSegment2 *seg) {
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.;
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];
}
}
@ -154,8 +159,12 @@ 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
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);
@ -182,16 +191,20 @@ 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) {
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 = sizeof(ImuData2); // 21
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);
unpack_imu_packet2(data + 4, &packet, is_correspondence_on);
res->packets.push_back(packet);
res->checksum = *(data + 4 + res->size);
}
@ -227,8 +240,8 @@ void Standard2ChannelsAdapter::GetImuResPacket(
}
void Standard2ChannelsAdapter::GetImuResPacket2(
const std::uint8_t *data, ImuResPacket2 *res) {
unpack_imu_res_packet2(data, res);
const std::uint8_t *data, ImuResPacket2 *res, bool is_correspondence_on) {
unpack_imu_res_packet2(data, res, is_correspondence_on);
}
MYNTEYE_END_NAMESPACE

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

@ -35,7 +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) override;
void GetImuResPacket2(
const std::uint8_t *data,
ImuResPacket2 *res,
bool is_correspondence_on) override;
};
MYNTEYE_END_NAMESPACE