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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/mynteye/types.h
View File

@ -441,10 +441,10 @@ struct MYNTEYE_API ImgData {
* IMU data.
*/
struct MYNTEYE_API ImuData {
/** Image frame id */
std::uint16_t frame_id;
/** accel or gyro flag:1 for accel,2 for gyro,3 for both */
std::uint8_t flag;
/** IMU timestamp in 0.01ms */
std::uint32_t timestamp;
std::uint64_t timestamp;
/** IMU accelerometer data for 3-axis: X, Y, Z. */
double accel[3];
/** IMU gyroscope data for 3-axis: X, Y, Z. */
@ -453,7 +453,7 @@ struct MYNTEYE_API ImuData {
double temperature;
void Reset() {
frame_id = 0;
flag = 0;
timestamp = 0;
std::fill(accel, accel + 3, 0);
std::fill(gyro, gyro + 3, 0);

6
samples/api/camera.cc
View File

@ -56,8 +56,7 @@ int main(int argc, char *argv[]) {
CHECK_NOTNULL(data.imu);
++imu_count;
VLOG(2) << "Imu count: " << imu_count;
VLOG(2) << " frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
VLOG(2) << ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
@ -107,8 +106,7 @@ int main(int argc, char *argv[]) {
auto &&motion_datas = api->GetMotionDatas();
motion_count += motion_datas.size();
for (auto &&data : motion_datas) {
LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
LOG(INFO) << ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]

18
samples/device/camera.cc
View File

@ -71,13 +71,12 @@ int main(int argc, char *argv[]) {
});
std::size_t imu_count = 0;
/*
device->SetMotionCallback([&imu_count](const device::MotionData &data) {
CHECK_NOTNULL(data.imu);
++imu_count;
VLOG(2) << "Imu count: " << imu_count;
VLOG(2) << " frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
VLOG(2) << ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
@ -86,7 +85,7 @@ int main(int argc, char *argv[]) {
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature;
});
*/
// Enable this will cache the motion datas until you get them.
// device->EnableMotionDatas();
device->Start(Source::VIDEO_STREAMING);
@ -99,12 +98,11 @@ int main(int argc, char *argv[]) {
device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT);
device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT);
/*
auto &&motion_datas = device->GetMotionDatas();
motion_count += motion_datas.size();
for (auto &&data : motion_datas) {
LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
LOG(INFO) << ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]
@ -113,7 +111,7 @@ int main(int argc, char *argv[]) {
<< ", gyro_z: " << data.imu->gyro[2]
<< ", temperature: " << data.imu->temperature;
}
*/
cv::Mat left_img(
left_data.frame->height(), left_data.frame->width(), CV_8UC2,
left_data.frame->data());
@ -146,7 +144,7 @@ int main(int argc, char *argv[]) {
<< ", fps: " << (1000.f * right_count / elapsed_ms);
LOG(INFO) << "Imu count: " << imu_count
<< ", hz: " << (1000.f * imu_count / elapsed_ms);
// LOG(INFO) << "Motion count: " << motion_count
// << ", hz: " << (1000.f * motion_count / elapsed_ms);
LOG(INFO) << "Motion count: " << motion_count
<< ", hz: " << (1000.f * motion_count / elapsed_ms);
return 0;
}

2
samples/tutorials/util/cv_painter.cc
View File

@ -118,7 +118,7 @@ cv::Rect CVPainter::DrawImuData(
if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
sign = -1;
Clear(ss) << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp
Clear(ss) << "stamp: " << data.timestamp
<< ", temp: " << fmt_temp << data.temperature;
cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);

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

4
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')
@ -69,6 +70,7 @@ bool unpack_stereo_img_data(
return false;
}
*/
img->frame_id = img_packet.frame_id;
img->timestamp = img_packet.timestamp;
img->exposure_time = img_packet.exposure_time;

64
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,26 +232,20 @@ 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
std::size_t seg_n = sizeof(ImuSegment); // 21
for(std::size_t i = 0; i < count; i++) {
segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
segments.push_back(ImuSegment(data + seg_n * i));
}
}
};
@ -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);
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);
i += 9 + (packet.count * seg_n);
}
//packet(2);
checksum = *(data + 4 + size);
}
};

4
wrappers/python/src/mynteye_py.cc
View File

@ -105,8 +105,7 @@ struct MYNTEYE_API MotionData {
ImuData imu;
bool operator==(const MotionData &other) const {
return imu.frame_id == other.imu.frame_id &&
imu.timestamp == other.imu.timestamp;
return imu.timestamp == other.imu.timestamp;
}
};
@ -365,7 +364,6 @@ BOOST_PYTHON_MODULE(mynteye_py) {
// bp::register_ptr_to_python<std::shared_ptr<ImgData>>();
bp::class_<ImuData>("ImuData")
.def_readonly("frame_id", &ImuData::frame_id)
.def_readonly("timestamp", &ImuData::timestamp)
.add_property(
"accel", +[](ImuData *o) { return array_ref<double>{o->accel}; })

2
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -26,7 +26,6 @@
<arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
<arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
<arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
<arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
<arg name="gravity" default="9.8" />
@ -116,7 +115,6 @@
<param name="points_frame_id" value="$(arg points_frame_id)" />
<param name="depth_frame_id" value="$(arg depth_frame_id)" />
<param name="imu_frame_id" value="$(arg imu_frame_id)" />
<param name="temp_frame_id" value="$(arg temp_frame_id)" />
<param name="gravity" value="$(arg gravity)" />

3
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -326,8 +326,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
NODELET_DEBUG_STREAM(
"Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
"Imu count: " << imu_count_<< ", timestamp: " << data.imu->timestamp
<< ", accel_x: " << data.imu->accel[0]
<< ", accel_y: " << data.imu->accel[1]
<< ", accel_z: " << data.imu->accel[2]