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

Get stream & motion datas in one thread

Browse Source
This commit is contained in:
John Zhao 2018-06-07 11:19:58 +08:00
parent 0286da50c6
commit 9b91304f0b
8 changed files with 105 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/device/device.cc
View File

@ -434,6 +434,7 @@ void Device::StartVideoStreaming() {
}
}
continuation();
OnStereoStreamUpdate();
// VLOG(2) << "Stereo video callback cost "
// << times::count<times::milliseconds>(times::now() - time_beg)
// << " ms";
@ -465,7 +466,7 @@ void Device::StartMotionTracking() {
}
motions_->SetMotionCallback(
std::bind(&Device::CallbackMotionData, this, std::placeholders::_1));
motions_->StartMotionTracking();
// motions_->StartMotionTracking();
motion_tracking_ = true;
}
@ -474,10 +475,12 @@ void Device::StopMotionTracking() {
LOG(WARNING) << "Cannot stop motion tracking without first starting it";
return;
}
motions_->StopMotionTracking();
// motions_->StopMotionTracking();
motion_tracking_ = false;
}
void Device::OnStereoStreamUpdate() {}
void Device::ReadAllInfos() {
device_info_ = std::make_shared<DeviceInfo>();

18
src/device/device.h
View File

@ -239,6 +239,18 @@ class MYNTEYE_API Device {
return device_;
}
std::shared_ptr<Streams> streams() const {
return streams_;
}
std::shared_ptr<Channels> channels() const {
return channels_;
}
std::shared_ptr<Motions> motions() const {
return motions_;
}
const StreamRequest &GetStreamRequest(const Capabilities &capability);
virtual void StartVideoStreaming();
@ -247,6 +259,8 @@ class MYNTEYE_API Device {
virtual void StartMotionTracking();
virtual void StopMotionTracking();
virtual void OnStereoStreamUpdate();
virtual std::vector<Stream> GetKeyStreams() const = 0;
bool video_streaming_;
@ -284,10 +298,6 @@ class MYNTEYE_API Device {
void CallbackPushedStreamData(const Stream &stream);
void CallbackMotionData(const device::MotionData &data);
std::shared_ptr<Channels> channels() {
return channels_;
}
friend API;
friend tools::DeviceWriter;
};

9
src/device/device_s.cc
View File

@ -15,6 +15,8 @@
#include <glog/logging.h>
#include "internal/motions.h"
MYNTEYE_BEGIN_NAMESPACE
StandardDevice::StandardDevice(std::shared_ptr<uvc::device> device)
@ -30,4 +32,11 @@ std::vector<Stream> StandardDevice::GetKeyStreams() const {
return {Stream::LEFT, Stream::RIGHT};
}
void StandardDevice::OnStereoStreamUpdate() {
if (motion_tracking_) {
auto &&motions = this->motions();
motions->DoMotionTrack();
}
}
MYNTEYE_END_NAMESPACE

2
src/device/device_s.h
View File

@ -28,6 +28,8 @@ class StandardDevice : public Device {
virtual ~StandardDevice();
std::vector<Stream> GetKeyStreams() const override;
void OnStereoStreamUpdate() override;
};
MYNTEYE_END_NAMESPACE

99
src/internal/channels.cc
View File

@ -31,7 +31,12 @@ MYNTEYE_BEGIN_NAMESPACE
namespace {
const uvc::xu mynteye_xu = {3, 2, {0x947a6d9f, 0x8a2f, 0x418d, {0x85, 0x9e, 0x6c, 0x9a, 0xa0, 0x38, 0x10, 0x14}}};
const uvc::xu mynteye_xu = {3,
2,
{0x947a6d9f,
0x8a2f,
0x418d,
{0x85, 0x9e, 0x6c, 0x9a, 0xa0, 0x38, 0x10, 0x14}}};
int XuCamCtrlId(Option option) {
switch (option) {
@ -276,6 +281,47 @@ void Channels::SetImuCallback(imu_callback_t callback) {
imu_callback_ = callback;
}
void Channels::DoImuTrack() {
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;
}
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) {
imu_callback_(packet);
}
}
res_packet.packets.clear();
}
void Channels::StartImuTracking(imu_callback_t callback) {
if (is_imu_tracking_) {
LOG(WARNING) << "Start imu tracking failed, is tracking already";
@ -287,8 +333,6 @@ void Channels::StartImuTracking(imu_callback_t callback) {
is_imu_tracking_ = true;
imu_track_thread_ = std::thread([this]() {
imu_sn_ = 0;
ImuReqPacket req_packet{imu_sn_};
ImuResPacket res_packet;
auto sleep = [](const times::system_clock::time_point &time_beg) {
auto &&time_elapsed_ms =
times::count<times::milliseconds>(times::now() - time_beg);
@ -301,48 +345,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
};
while (!imu_track_stop_) {
auto &&time_beg = times::now();
req_packet.serial_number = imu_sn_;
if (!XuImuWrite(req_packet)) {
sleep(time_beg);
continue;
}
if (!XuImuRead(&res_packet)) {
sleep(time_beg);
continue;
}
if (res_packet.packets.size() == 0) {
sleep(time_beg);
continue;
}
VLOG(2) << "Imu req sn: " << imu_sn_
<< ", res count: " << [&res_packet]() {
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";
sleep(time_beg);
continue;
}
imu_sn_ = sn;
if (imu_callback_) {
for (auto &&packet : res_packet.packets) {
imu_callback_(packet);
}
}
res_packet.packets.clear();
DoImuTrack();
sleep(time_beg);
}
});
@ -870,7 +873,8 @@ bool Channels::PuControlQuery(
}
bool Channels::XuControlRange(
channel_t channel, uint8_t id, int32_t *min, int32_t *max, int32_t *def) const {
channel_t channel, uint8_t id, int32_t *min, int32_t *max,
int32_t *def) const {
return XuControlRange(mynteye_xu, channel, id, min, max, def);
}
@ -1016,7 +1020,8 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const {
int id = XuCamCtrlId(option);
int32_t min = 0, max = 0, def = 0;
if (!XuControlRange(CHANNEL_CAM_CTRL, static_cast<std::uint8_t>(id), &min, &max, &def)) {
if (!XuControlRange(
CHANNEL_CAM_CTRL, static_cast<std::uint8_t>(id), &min, &max, &def)) {
LOG(WARNING) << "Get XuControlInfo of " << option << " failed";
}
return {min, max, def};

9
src/internal/channels.h
View File

@ -95,6 +95,8 @@ class MYNTEYE_API Channels {
bool RunControlAction(const Option &option) const;
void SetImuCallback(imu_callback_t callback);
void DoImuTrack();
void StartImuTracking(imu_callback_t callback = nullptr);
void StopImuTracking();
@ -111,10 +113,11 @@ class MYNTEYE_API Channels {
bool PuControlQuery(Option option, uvc::pu_query query, int32_t *value) const;
bool XuControlRange(
channel_t channel, uint8_t id, int32_t *min, int32_t *max, int32_t *def) const;
bool XuControlRange(
const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min, int32_t *max,
channel_t channel, uint8_t id, int32_t *min, int32_t *max,
int32_t *def) const;
bool XuControlRange(
const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min,
int32_t *max, int32_t *def) const;
bool XuControlQuery(
channel_t channel, uvc::xu_query query, uint16_t size,

24
src/internal/motions.cc
View File

@ -34,11 +34,8 @@ Motions::~Motions() {
void Motions::SetMotionCallback(motion_callback_t callback) {
motion_callback_ = callback;
}
void Motions::StartMotionTracking() {
if (!is_imu_tracking) {
channels_->StartImuTracking([this](const ImuPacket &packet) {
if (motion_callback_) {
channels_->SetImuCallback([this](const ImuPacket &packet) {
if (!motion_callback_ && !motion_datas_enabled_) {
LOG(WARNING) << "";
return;
@ -62,11 +59,22 @@ void Motions::StartMotionTracking() {
std::lock_guard<std::mutex> _(mtx_datas_);
motion_data_t data = {imu};
motion_datas_.push_back(data);
if (motion_callback_) {
motion_callback_(data);
}
motion_callback_(data);
}
});
} else {
channels_->SetImuCallback(nullptr);
}
}
void Motions::DoMotionTrack() {
channels_->DoImuTrack();
}
void Motions::StartMotionTracking() {
if (!is_imu_tracking) {
channels_->StartImuTracking();
is_imu_tracking = true;
} else {
LOG(WARNING) << "Imu is tracking already";

1
src/internal/motions.h
View File

@ -37,6 +37,7 @@ class Motions {
~Motions();
void SetMotionCallback(motion_callback_t callback);
void DoMotionTrack();
void StartMotionTracking();
void StopMotionTracking();