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

change the data protocol of imu

Browse Source
This commit is contained in:
KalmanSLightech
2018-07-21 15:34:07 +08:00
parent 25220a27ed
commit b33fc86a45
13 changed files with 76 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/api/api.h
View File

@@ -69,8 +69,7 @@ struct MYNTEYE_API MotionData {
bool operator==(const MotionData &other) const {
if (imu && other.imu) {
return imu->frame_id == other.imu->frame_id &&
imu->timestamp == other.imu->timestamp;
return imu->timestamp == other.imu->timestamp;
}
return false;
}

4
src/device/device.cc
View File

@@ -103,14 +103,14 @@ std::shared_ptr<Device> Device::Create(
std::string model_s = name.substr(9,5);
VLOG(2) << "MYNE EYE Model: " << model_s;
DeviceModel model(model_s);
if(model.type == 's') {
if(model.type == 'S') {
switch (model.custom_code) {
case '0':
return std::make_shared<StandardDevice>(device);
case 'A':
return std::make_shared<StandardDevice>(device);
default:
LOG(FATAL) << "No such custom code now"
LOG(FATAL) << "No such custom code now";
}
} else {
LOG(FATAL) << "MYNT EYE model is not supported now";

2
src/internal/channels.cc
View File

@@ -306,7 +306,7 @@ void Channels::DoImuTrack() {
return n;
}();
auto &&sn = res_packet.packets.back().serial_number;
auto &&sn = res_packet.packets.back().segments.back().serial_number;
if (imu_sn_ == sn) {
VLOG(2) << "New imu not ready, dropped";
return;

22
src/internal/motions.cc
View File

@@ -42,20 +42,26 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
}
for (auto &&seg : packet.segments) {
auto &&imu = std::make_shared<ImuData>();
imu->frame_id = seg.frame_id;
// imu->frame_id = seg.frame_id;
// if (seg.offset < 0 &&
// static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
// LOG(WARNING) << "Imu timestamp offset is incorrect";
// }
imu->timestamp = packet.timestamp + seg.offset;
imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
imu->timestamp = seg.timestamp;
imu->flag = seg.flag;
imu->temperature = seg.temperature / 326.8f + 25;
if((imu->flag) & 0x01 != 0) {
imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000;
imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000;
imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000;
}
if((imu->flag) & 0x02 != 0) {
imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000;
imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000;
imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000;
}
std::lock_guard<std::mutex> _(mtx_datas_);
motion_data_t data = {imu};
motion_datas_.push_back(data);

6
src/internal/streams.cc
View File

@@ -48,7 +48,7 @@ bool unpack_stereo_img_data(
// << ", timestamp="<< std::dec << img_packet.timestamp
// << ", exposure_time="<< std::dec << img_packet.exposure_time
// << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
/*
if (img_packet.header != 0x3B) {
LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
@@ -60,6 +60,7 @@ bool unpack_stereo_img_data(
for (std::size_t i = 2, n = packet_n - 2; i <= n; i++) { // content: [2,9]
checksum = (checksum ^ packet[i]);
}
/*
if (img_packet.checksum != checksum) {
LOG(WARNING) << "Image packet checksum should be 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
@@ -68,7 +69,8 @@ bool unpack_stereo_img_data(
<< static_cast<int>(checksum) << " now";
return false;
}
*/
*/
img->frame_id = img_packet.frame_id;
img->timestamp = img_packet.timestamp;
img->exposure_time = img_packet.exposure_time;

72
src/internal/types.h
View File

@@ -148,7 +148,7 @@ struct ImagePacket {
std::uint8_t header;
std::uint8_t size;
std::uint16_t frame_id;
std::uint32_t timestamp;
std::uint64_t timestamp;
std::uint16_t exposure_time;
std::uint8_t checksum;
@@ -161,10 +161,11 @@ struct ImagePacket {
header = *data;
size = *(data + 1);
frame_id = (*(data + 2) << 8) | *(data + 3);
timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
*(data + 7);
exposure_time = (*(data + 8) << 8) | *(data + 9);
checksum = *(data + 10);
timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
(*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
exposure_time = (*(data + 12) << 8) | *(data + 13);
checksum = *(data + 14);
}
};
#pragma pack(pop)
@@ -199,11 +200,11 @@ struct ImuReqPacket {
*/
#pragma pack(push, 1)
struct ImuSegment {
std::int16_t offset;
std::uint16_t frame_id;
std::int16_t accel[3];
std::uint32_t serial_number;
std::uint64_t timestamp;
std::uint8_t flag;
std::int16_t temperature;
std::int16_t gyro[3];
std::int16_t aceel_or_gyro[3];
ImuSegment() = default;
explicit ImuSegment(std::uint8_t *data) {
@@ -211,15 +212,16 @@ struct ImuSegment {
}
void from_data(std::uint8_t *data) {
offset = (*(data) << 8) | *(data + 1);
frame_id = (*(data + 2) << 8) | *(data + 3);
accel[0] = (*(data + 4) << 8) | *(data + 5);
accel[1] = (*(data + 6) << 8) | *(data + 7);
accel[2] = (*(data + 8) << 8) | *(data + 9);
temperature = (*(data + 10) << 8) | *(data + 11);
gyro[0] = (*(data + 12) << 8) | *(data + 13);
gyro[1] = (*(data + 14) << 8) | *(data + 15);
gyro[2] = (*(data + 16) << 8) | *(data + 17);
serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
*(data + 3);
timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
(*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
flag = *(data + 12);
temperature = (*(data + 13) << 8) | *(data + 14);
aceel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
aceel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
aceel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
}
};
#pragma pack(pop)
@@ -230,27 +232,21 @@ struct ImuSegment {
*/
#pragma pack(push, 1)
struct ImuPacket {
std::uint32_t serial_number;
std::uint32_t timestamp;
std::uint8_t count;
std::vector<ImuSegment> segments;
ImuPacket() = default;
explicit ImuPacket(std::uint8_t *data) {
explicit ImuPacket(std::uint8_t seg_count,std::uint8_t *data) {
count = seg_count;
from_data(data);
}
void from_data(std::uint8_t *data) {
serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
*(data + 3);
timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
*(data + 7);
count = *(data + 8);
std::size_t seg_n = sizeof(ImuSegment); // 18
for (std::size_t i = 0; i < count; i++) {
segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
}
std::size_t seg_n = sizeof(ImuSegment); // 21
for(std::size_t i = 0; i < count; i++) {
segments.push_back(ImuSegment(data + seg_n * i));
}
}
};
#pragma pack(pop)
@@ -277,13 +273,11 @@ struct ImuResPacket {
state = *(data + 1);
size = (*(data + 2) << 8) | *(data + 3);
std::size_t seg_n = sizeof(ImuSegment); // 18
for (std::size_t i = 4; i < size;) {
ImuPacket packet(data + i);
packets.push_back(packet);
i += 9 + (packet.count * seg_n);
}
std::size_t seg_n = sizeof(ImuSegment); // 21
std::uint8_t seg_count = (size - 4) / seg_n;
ImuPacket packet(seg_count,data + 4);
packets.push_back(packet);
//packet(2);
checksum = *(data + 4 + size);
}
};