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

test(timestamp): s2 stamp use normal logic.

Browse Source
This commit is contained in:
TinyO 2019-10-17 11:51:19 +08:00
parent e016a17813
commit ea11b4b548
7 changed files with 174 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -141,6 +141,7 @@ Channels::Channels(const std::shared_ptr<uvc::device> &device,
adapter_(adapter),
is_imu_tracking_(false),
is_imu_proto2_(false),
is_s2_(false),
enable_imu_correspondence(false),
imu_track_stop_(false),
imu_sn_(0),
@ -422,7 +423,11 @@ void Channels::DoImuTrack() {
if (IsImuProtocol2()) {
return DoImuTrack2();
} else {
return DoImuTrack1();
if (IsS2()) {
return DoImuTrack1();
} else {
return DoImuTrack1WithTimeLimmitFix();
}
}
}
@ -462,6 +467,50 @@ void Channels::DoImuTrack1() {
}
imu_sn_ = sn;
if (imu_callback_) {
for (auto &packet : res_packet.packets) {
imu_callback_(to_pak2(packet, accel_range, gyro_range));
}
}
res_packet.packets.clear();
}
void Channels::DoImuTrack1WithTimeLimmitFix() {
static ImuReqPacket req_packet{0};
static ImuResPacket res_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) {
auto pak2_tmp = to_pak2(packet, accel_range, gyro_range);
@ -522,7 +571,6 @@ void Channels::DoImuTrack2() {
imu_sn_ = sn;
if (imu_callback_) {
for (auto &packet : res_packet.packets) {
CheckTimeStampLimmit(packet);
imu_callback_(packet);
}
}
@ -559,10 +607,18 @@ void Channels::StartImuTracking(imu_callback_t callback) {
sleep(time_beg);
}
} else {
while (!imu_track_stop_) {
auto &&time_beg = times::now();
DoImuTrack1();
sleep(time_beg);
if (IsS2()) {
while (!imu_track_stop_) {
auto &&time_beg = times::now();
DoImuTrack1();
sleep(time_beg);
}
} else {
while (!imu_track_stop_) {
auto &&time_beg = times::now();
DoImuTrack1WithTimeLimmitFix();
sleep(time_beg);
}
}
}
});
@ -643,6 +699,7 @@ bool Channels::GetFiles(
is_imu_proto2_ = dev_info_ &&
dev_info_->spec_version >= Version(1, 3) &&
strstr(dev_info_->name.c_str(), "S2") != nullptr;
is_s2_ = strstr(dev_info_->name.c_str(), "S2") != nullptr;
} break;
case FID_IMG_PARAMS: {
if (file_size > 0) {

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

@ -82,6 +82,7 @@ class MYNTEYE_API Channels {
void SetImuCallback(imu_callback_t callback);
void DoImuTrack();
void DoImuTrack1();
void DoImuTrack1WithTimeLimmitFix();
void DoImuTrack2();
void StartImuTracking(imu_callback_t callback = nullptr);
@ -92,6 +93,7 @@ class MYNTEYE_API Channels {
bool SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
inline bool IsImuProtocol2() const { return is_imu_proto2_; }
inline bool IsS2() const { return is_s2_; }
inline void EnableImuCorrespondence(bool is_enable) {
enable_imu_correspondence = is_enable;
}
@ -145,6 +147,7 @@ class MYNTEYE_API Channels {
bool is_imu_tracking_;
bool is_imu_proto2_;
bool is_s2_;
bool enable_imu_correspondence;
std::thread imu_track_thread_;
volatile bool imu_track_stop_;

79
src/mynteye/device/device.cc
View File

@ -534,32 +534,61 @@ void Device::StartVideoStreaming() {
auto &&stream_request = GetStreamRequest(stream_cap);
streams_->ConfigStream(stream_cap, stream_request);
uvc::set_device_mode(
*device_, stream_request.width, stream_request.height,
static_cast<int>(stream_request.format), stream_request.fps,
[this, stream_cap](
const void *data, std::function<void()> continuation) {
// drop the first stereo stream data
static std::uint8_t drop_count = 1;
if (drop_count > 0) {
--drop_count;
continuation();
return;
}
// auto &&time_beg = times::now();
{
std::lock_guard<std::mutex> _(mtx_streams_);
if (streams_->PushStream(stream_cap, data)) {
CallbackPushedStreamData(Stream::LEFT);
CallbackPushedStreamData(Stream::RIGHT);
if (strstr(this->device_info_->name.c_str(), "S2") == nullptr) {
uvc::set_device_mode(
*device_, stream_request.width, stream_request.height,
static_cast<int>(stream_request.format), stream_request.fps,
[this, stream_cap](
const void *data, std::function<void()> continuation) {
// drop the first stereo stream data
static std::uint8_t drop_count = 1;
if (drop_count > 0) {
--drop_count;
continuation();
return;
}
}
continuation();
OnStereoStreamUpdate();
// VLOG(2) << "Stereo video callback cost "
// << times::count<times::milliseconds>(times::now() - time_beg)
// << " ms";
});
// auto &&time_beg = times::now();
{
std::lock_guard<std::mutex> _(mtx_streams_);
if (streams_->PushStreamS1(stream_cap, data)) {
CallbackPushedStreamData(Stream::LEFT);
CallbackPushedStreamData(Stream::RIGHT);
}
}
continuation();
OnStereoStreamUpdate();
// VLOG(2) << "Stereo video callback cost "
// << times::count<times::milliseconds>(times::now() - time_beg)
// << " ms";
});
} else {
uvc::set_device_mode(
*device_, stream_request.width, stream_request.height,
static_cast<int>(stream_request.format), stream_request.fps,
[this, stream_cap](
const void *data, std::function<void()> continuation) {
// drop the first stereo stream data
static std::uint8_t drop_count = 1;
if (drop_count > 0) {
--drop_count;
continuation();
return;
}
// auto &&time_beg = times::now();
{
std::lock_guard<std::mutex> _(mtx_streams_);
if (streams_->PushStream(stream_cap, data)) {
CallbackPushedStreamData(Stream::LEFT);
CallbackPushedStreamData(Stream::RIGHT);
}
}
continuation();
OnStereoStreamUpdate();
// VLOG(2) << "Stereo video callback cost "
// << times::count<times::milliseconds>(times::now() - time_beg)
// << " ms";
});
}
} else {
LOG(FATAL) << "Not any stream capabilities are supported by this device";
}

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

@ -15,6 +15,8 @@
#include "mynteye/logger.h"
#define DEBUG_TIME_LIMIT true
MYNTEYE_BEGIN_NAMESPACE
namespace {
@ -43,6 +45,9 @@ struct ImuData {
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
#if DEBUG_TIME_LIMIT
timestamp += 4200000000;
#endif
flag = *(data + 12);
temperature = (*(data + 13) << 8) | *(data + 14);
accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);

5
src/mynteye/device/standard2/streams_adapter_s2.cc
View File

@ -18,6 +18,8 @@
#include "mynteye/logger.h"
#include "mynteye/device/types.h"
#define DEBUG_TIME_LIMIT true
MYNTEYE_BEGIN_NAMESPACE
namespace {
@ -51,6 +53,9 @@ struct ImagePacket {
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
#if DEBUG_TIME_LIMIT
timestamp += 4200000000;
#endif
exposure_time = (*(data + 12) << 8) | *(data + 13);
checksum = *(data + 14);
}

43
src/mynteye/device/streams.cc
View File

@ -66,6 +66,49 @@ void Streams::CheckTimeStampLimmit(std::shared_ptr<ImgData> img) {
}
bool Streams::PushStream(const Capabilities &capability, const void *data) {
if (!HasStreamConfigRequest(capability)) {
LOG(FATAL) << "Cannot push stream without stream config request";
}
std::unique_lock<std::mutex> lock(mtx_);
auto &&request = GetStreamConfigRequest(capability);
bool pushed = false;
switch (capability) {
case Capabilities::STEREO:
case Capabilities::STEREO_COLOR: {
// alloc left
AllocStreamData(capability, Stream::LEFT, request);
auto &&left_data = stream_datas_map_[Stream::LEFT].back();
// unpack img data
if (unpack_img_data_map_[Stream::LEFT](
data, request, left_data.img.get())) {
left_data.frame_id = left_data.img->frame_id;
// alloc right
AllocStreamData(capability, Stream::RIGHT, request);
auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
*right_data.img = *left_data.img;
right_data.frame_id = left_data.img->frame_id;
// unpack frame
unpack_img_pixels_map_[Stream::LEFT](
data, request, left_data.frame.get());
unpack_img_pixels_map_[Stream::RIGHT](
data, request, right_data.frame.get());
pushed = true;
} else {
// discard left
DiscardStreamData(Stream::LEFT);
VLOG(2) << "Image packet is unaccepted, frame dropped";
pushed = false;
}
} break;
default:
LOG(FATAL) << "Not supported " << capability << " now";
}
if (HasKeyStreamDatas())
cv_.notify_one();
return pushed;
}
bool Streams::PushStreamS1(const Capabilities &capability, const void *data) {
if (!HasStreamConfigRequest(capability)) {
LOG(FATAL) << "Cannot push stream without stream config request";
}

1
src/mynteye/device/streams.h
View File

@ -49,6 +49,7 @@ class Streams {
const Capabilities &capability, const StreamRequest &request);
bool PushStream(const Capabilities &capability, const void *data);
bool PushStreamS1(const Capabilities &capability, const void *data);
void WaitForStreams();