diff --git a/src/mynteye/device/channels.cc b/src/mynteye/device/channels.cc index 30de25b..d3db8ff 100644 --- a/src/mynteye/device/channels.cc +++ b/src/mynteye/device/channels.cc @@ -120,13 +120,15 @@ void CheckSpecVersion(const Version *spec_version) { } // namespace -Channels::Channels(std::shared_ptr device) +Channels::Channels(const Model &model, std::shared_ptr device) : device_(device), + model_(model), is_imu_tracking_(false), imu_track_stop_(false), imu_sn_(0), imu_callback_(nullptr) { VLOG(2) << __func__; + imu_res_version_ = (model == Model::STANDARD) ? 1 : 2; // UpdateControlInfos(); } @@ -327,13 +329,17 @@ bool Channels::RunControlAction(const Option &option) const { } } +std::uint8_t Channels::GetImuResVersion() { + return imu_res_version_; +} + void Channels::SetImuCallback(imu_callback_t callback) { imu_callback_ = callback; } void Channels::DoImuTrack() { static ImuReqPacket req_packet{0}; - static ImuResPacket res_packet(model_); + static ImuResPacket res_packet(imu_res_version_); req_packet.serial_number = imu_sn_; if (!XuImuWrite(req_packet)) { diff --git a/src/mynteye/device/channels.h b/src/mynteye/device/channels.h index b55af5d..29ec704 100644 --- a/src/mynteye/device/channels.h +++ b/src/mynteye/device/channels.h @@ -71,9 +71,8 @@ class MYNTEYE_API Channels { using img_params_t = std::map; using imu_params_t = device::imu_params_t; - std::uint8_t model_; - - explicit Channels(std::shared_ptr device); + explicit Channels( + const Model &model, std::shared_ptr device); ~Channels(); void LogControlInfos() const; @@ -85,6 +84,8 @@ class MYNTEYE_API Channels { bool RunControlAction(const Option &option) const; + std::uint8_t GetImuResVersion(); + void SetImuCallback(imu_callback_t callback); void DoImuTrack(); @@ -131,6 +132,10 @@ class MYNTEYE_API Channels { control_info_t PuControlInfo(Option option) const; control_info_t XuControlInfo(Option option) const; + std::uint8_t imu_res_version_; + + Model model_; + std::shared_ptr device_; std::map control_infos_; diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index c63cdbe..356565e 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -85,7 +85,7 @@ Device::Device(const Model &model, std::shared_ptr device) model_(model), device_(device), streams_(nullptr), - channels_(std::make_shared(device)), + channels_(std::make_shared(model_, device)), motions_(std::make_shared(channels_)) { VLOG(2) << __func__; ReadAllInfos(); diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index c7d2e6e..bd39b8b 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -36,7 +36,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) { if (motion_callback_) { accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE); if (accel_range == -1) - accel_range = (channels_->model_) ? 8 : 12; // ugly + accel_range = (channels_->GetImuResVersion() == 1) ? 8 : 12; gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE); if (gyro_range == -1) @@ -78,8 +78,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) { } } -void Motions::DoMotionTrack(std::uint8_t model) { - channels_->model_ = model; +void Motions::DoMotionTrack() { channels_->DoImuTrack(); } diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h index 129727b..9bb50d3 100644 --- a/src/mynteye/device/motions.h +++ b/src/mynteye/device/motions.h @@ -37,7 +37,7 @@ class Motions { ~Motions(); void SetMotionCallback(motion_callback_t callback); - void DoMotionTrack(std::uint8_t model); + void DoMotionTrack(); void StartMotionTracking(); void StopMotionTracking(); @@ -58,8 +58,8 @@ class Motions { std::mutex mtx_datas_; - int accel_range = 12; - int gyro_range = 1000; + int accel_range; + int gyro_range; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard/device_s.cc b/src/mynteye/device/standard/device_s.cc index 7c4fddd..36719ba 100644 --- a/src/mynteye/device/standard/device_s.cc +++ b/src/mynteye/device/standard/device_s.cc @@ -44,7 +44,7 @@ void StandardDevice::StartVideoStreaming() { void StandardDevice::OnStereoStreamUpdate() { if (motion_tracking_) { auto &&motions = this->motions(); - motions->DoMotionTrack(1); + motions->DoMotionTrack(); } } diff --git a/src/mynteye/device/standard2/device_s2.cc b/src/mynteye/device/standard2/device_s2.cc index 96f99d6..4b5fc44 100644 --- a/src/mynteye/device/standard2/device_s2.cc +++ b/src/mynteye/device/standard2/device_s2.cc @@ -39,7 +39,7 @@ std::shared_ptr Standard2Device::CreateStreamsAdapter() const { void Standard2Device::OnStereoStreamUpdate() { if (motion_tracking_) { auto &&motions = this->motions(); - motions->DoMotionTrack(2); + motions->DoMotionTrack(); } } diff --git a/src/mynteye/device/types.h b/src/mynteye/device/types.h index a454857..cd216b0 100644 --- a/src/mynteye/device/types.h +++ b/src/mynteye/device/types.h @@ -322,9 +322,9 @@ struct ImuSegment { gyro[2] = (flag == 2) ? group.accel_or_gyro[2] : 0; } - explicit ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) { + ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) { frame_id = static_cast (group.frame_id); - this->timestamp = static_cast (timestamp + group.offset); + this->timestamp = static_cast (timestamp + group.offset) * 10; flag = 3; temperature = group.temperature; accel[0] = group.accel[0]; @@ -343,19 +343,19 @@ struct ImuSegment { */ #pragma pack(push, 1) struct ImuPacket { - std::uint8_t model; + std::uint8_t version; std::uint8_t count; std::uint32_t serial_number; std::vector segments; ImuPacket() = default; explicit ImuPacket( - std::uint8_t model, std::uint8_t seg_count, std::uint8_t *data) - : model(model), count(seg_count) { + std::uint8_t version, std::uint8_t seg_count, std::uint8_t *data) + : version(version), count(seg_count) { from_data(data); } void from_data(std::uint8_t *data) { - if (model == 1) { + if (version == 1) { serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) | *(data + 3); @@ -369,7 +369,7 @@ struct ImuPacket { ImuGroupS1 group(data + 9 + (seg_n * i)); segments.push_back(ImuSegment(timestamp, group)); } - } else if (model == 2) { + } else if (version == 2) { std::size_t seg_n = sizeof(ImuGroupS2); // 21 for (std::size_t i = 0; i < count; i++) { ImuGroupS2 group(data + seg_n * i); @@ -387,7 +387,7 @@ struct ImuPacket { */ #pragma pack(push, 1) struct ImuResPacket { - std::uint8_t model; + std::uint8_t version; std::uint8_t header; std::uint8_t state; std::uint16_t size; @@ -395,27 +395,27 @@ struct ImuResPacket { std::uint8_t checksum; ImuResPacket() = default; - explicit ImuResPacket(std::uint8_t model) : model(model) {} + explicit ImuResPacket(std::uint8_t version) : version(version) {} void from_data(std::uint8_t *data) { header = *data; state = *(data + 1); size = (*(data + 2) << 8) | *(data + 3); - if (model == 1) { + if (version == 1) { std::size_t seg_n = sizeof(ImuGroupS1); // 18 for (std::size_t i = 4; i < size;) { - ImuPacket packet(model, 0, data + i); + ImuPacket packet(version, 0, data + i); packets.push_back(packet); i += 9 + (packet.count * seg_n); } checksum = *(data + 4 + size); } - if (model == 2) { + if (version == 2) { std::size_t seg_n = sizeof(ImuGroupS2); // 21 std::uint8_t seg_count = size / seg_n; - ImuPacket packet(model, seg_count, data + 4); + ImuPacket packet(version, seg_count, data + 4); packets.push_back(packet); // packet(2); checksum = *(data + 4 + size);