diff --git a/src/device/device.cc b/src/device/device.cc index c42812e..28d18d5 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -434,6 +434,7 @@ void Device::StartVideoStreaming() { } } continuation(); + OnStereoStreamUpdate(); // VLOG(2) << "Stereo video callback cost " // << times::count(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(); diff --git a/src/device/device.h b/src/device/device.h index a4dfbb4..4a5d258 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -239,6 +239,18 @@ class MYNTEYE_API Device { return device_; } + std::shared_ptr streams() const { + return streams_; + } + + std::shared_ptr channels() const { + return channels_; + } + + std::shared_ptr 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 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() { - return channels_; - } - friend API; friend tools::DeviceWriter; }; diff --git a/src/device/device_s.cc b/src/device/device_s.cc index ac23e3d..fdffe93 100644 --- a/src/device/device_s.cc +++ b/src/device/device_s.cc @@ -15,6 +15,8 @@ #include +#include "internal/motions.h" + MYNTEYE_BEGIN_NAMESPACE StandardDevice::StandardDevice(std::shared_ptr device) @@ -30,4 +32,11 @@ std::vector StandardDevice::GetKeyStreams() const { return {Stream::LEFT, Stream::RIGHT}; } +void StandardDevice::OnStereoStreamUpdate() { + if (motion_tracking_) { + auto &&motions = this->motions(); + motions->DoMotionTrack(); + } +} + MYNTEYE_END_NAMESPACE diff --git a/src/device/device_s.h b/src/device/device_s.h index 4e08094..2a600b4 100644 --- a/src/device/device_s.h +++ b/src/device/device_s.h @@ -28,6 +28,8 @@ class StandardDevice : public Device { virtual ~StandardDevice(); std::vector GetKeyStreams() const override; + + void OnStereoStreamUpdate() override; }; MYNTEYE_END_NAMESPACE diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 75e322c..d09d976 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -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::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(id), &min, &max, &def)) { + if (!XuControlRange( + CHANNEL_CAM_CTRL, static_cast(id), &min, &max, &def)) { LOG(WARNING) << "Get XuControlInfo of " << option << " failed"; } return {min, max, def}; diff --git a/src/internal/channels.h b/src/internal/channels.h index 6588b92..c0f9de0 100644 --- a/src/internal/channels.h +++ b/src/internal/channels.h @@ -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, diff --git a/src/internal/motions.cc b/src/internal/motions.cc index 3bd346c..7183c6a 100644 --- a/src/internal/motions.cc +++ b/src/internal/motions.cc @@ -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 _(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"; diff --git a/src/internal/motions.h b/src/internal/motions.h index 7c7e809..0472605 100644 --- a/src/internal/motions.h +++ b/src/internal/motions.h @@ -37,6 +37,7 @@ class Motions { ~Motions(); void SetMotionCallback(motion_callback_t callback); + void DoMotionTrack(); void StartMotionTracking(); void StopMotionTracking();