diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 0ab79fd..7f3f9b3 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -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); diff --git a/samples/api/camera.cc b/samples/api/camera.cc index 5cddd85..02ad7c9 100644 --- a/samples/api/camera.cc +++ b/samples/api/camera.cc @@ -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] @@ -106,9 +105,8 @@ 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 + for (auto &&data : motion_datas) { + LOG(INFO) << ", timestamp: " << data.imu->timestamp << ", accel_x: " << data.imu->accel[0] << ", accel_y: " << data.imu->accel[1] << ", accel_z: " << data.imu->accel[2] diff --git a/samples/device/camera.cc b/samples/device/camera.cc index 5815cbd..65d3cee 100644 --- a/samples/device/camera.cc +++ b/samples/device/camera.cc @@ -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) << "Imu count: " << imu_count; + 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 + for (auto &&data : motion_datas) { + 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; } diff --git a/samples/tutorials/util/cv_painter.cc b/samples/tutorials/util/cv_painter.cc index c0c6f6a..83fa767 100644 --- a/samples/tutorials/util/cv_painter.cc +++ b/samples/tutorials/util/cv_painter.cc @@ -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); diff --git a/src/api/api.h b/src/api/api.h index a7f0ff8..8a259a3 100644 --- a/src/api/api.h +++ b/src/api/api.h @@ -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; } diff --git a/src/device/device.cc b/src/device/device.cc index 2c37017..c38db1d 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -103,14 +103,14 @@ std::shared_ptr 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(device); case 'A': return std::make_shared(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"; diff --git a/src/internal/channels.cc b/src/internal/channels.cc index d09d976..88558ef 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -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; diff --git a/src/internal/motions.cc b/src/internal/motions.cc index 7183c6a..881ac5f 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -42,20 +42,26 @@ void Motions::SetMotionCallback(motion_callback_t callback) { } for (auto &&seg : packet.segments) { auto &&imu = std::make_shared(); - imu->frame_id = seg.frame_id; + // imu->frame_id = seg.frame_id; // if (seg.offset < 0 && // static_cast(-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 _(mtx_datas_); motion_data_t data = {imu}; motion_datas_.push_back(data); diff --git a/src/internal/streams.cc b/src/internal/streams.cc index d97a656..1a78513 100644 --- a/src/internal/streams.cc +++ b/src/internal/streams.cc @@ -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(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(checksum) << " now"; return false; } -*/ + */ + img->frame_id = img_packet.frame_id; img->timestamp = img_packet.timestamp; img->exposure_time = img_packet.exposure_time; diff --git a/src/internal/types.h b/src/internal/types.h index 216fe89..ac5992d 100644 --- a/src/internal/types.h +++ b/src/internal/types.h @@ -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 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); } }; diff --git a/wrappers/python/src/mynteye_py.cc b/wrappers/python/src/mynteye_py.cc index 8107f55..3bcccf4 100644 --- a/wrappers/python/src/mynteye_py.cc +++ b/wrappers/python/src/mynteye_py.cc @@ -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>(); bp::class_("ImuData") - .def_readonly("frame_id", &ImuData::frame_id) .def_readonly("timestamp", &ImuData::timestamp) .add_property( "accel", +[](ImuData *o) { return array_ref{o->accel}; }) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 83e1e70..c906c32 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -26,7 +26,6 @@ - @@ -116,7 +115,6 @@ - diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 5e7d153..cb2a9bb 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -325,9 +325,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ++imu_count_; 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 + NODELET_DEBUG_STREAM( + "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]