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

refactor(device): add channels adapter to device

Browse Source
This commit is contained in:
John Zhao 2018-12-24 00:39:43 +08:00
parent c6bed032cb
commit 2f4ffded7c
16 changed files with 455 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -172,8 +172,10 @@ set(MYNTEYE_SRCS
src/mynteye/device/context.cc
src/mynteye/device/device.cc
src/mynteye/device/motions.cc
src/mynteye/device/standard/channels_adapter_s.cc
src/mynteye/device/standard/device_s.cc
src/mynteye/device/standard/streams_adapter_s.cc
src/mynteye/device/standard2/channels_adapter_s2.cc
src/mynteye/device/standard2/device_s2.cc
src/mynteye/device/standard2/streams_adapter_s2.cc
src/mynteye/device/streams.cc

10
include/mynteye/device/device.h
View File

@ -43,6 +43,7 @@ struct DeviceInfo;
class API;
class Channels;
class ChannelsAdapter;
class Motions;
class Streams;
class StreamsAdapter;
@ -70,7 +71,13 @@ class MYNTEYE_API Device {
using img_params_t = device::img_params_t;
using imu_params_t = device::imu_params_t;
Device(const Model &model, std::shared_ptr<uvc::device> device);
protected:
Device(const Model &model,
const std::shared_ptr<uvc::device> &device,
const std::shared_ptr<StreamsAdapter> &streams_adapter,
const std::shared_ptr<ChannelsAdapter> &channels_adapter);
public:
virtual ~Device();
/**
@ -301,7 +308,6 @@ class MYNTEYE_API Device {
virtual void OnStereoStreamUpdate();
virtual Capabilities GetKeyStreamCapability() const = 0;
virtual std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const = 0;
std::map<Resolution, device::img_params_t> GetImgParams() const {
return all_img_params_;

12
include/mynteye/types.h
View File

@ -212,7 +212,7 @@ enum class Option : std::uint8_t {
* The range of accelerometer
*
* value of standard 1: {4,8,16,32}, default: 8
* value of standard 2: {6,12,24,48}, default: 6
* value of standard 2: {6,12,24,48}, default: 12
*/
ACCELEROMETER_RANGE,
/**
@ -357,7 +357,7 @@ struct MYNTEYE_API StreamRequest {
std::uint16_t height;
/** Stream pixel format */
Format format;
/** Stream frames per second (unused) */
/** Stream frames per second */
std::uint16_t fps;
StreamRequest() {}
@ -522,7 +522,13 @@ struct MYNTEYE_API ImgData {
struct MYNTEYE_API ImuData {
/** IMU frame id */
std::uint32_t frame_id;
/** IMU accel or gyro flag: 1 for accel, 2 for gyro, 3 for both */
/**
* IMU accel or gyro flag
*
* 0: accel and gyro are both valid
* 1: accel is valid
* 2: gyro is valid
*/
std::uint8_t flag;
/** IMU timestamp in 1us */
std::uint64_t timestamp;

109
src/mynteye/device/channels.cc
View File

@ -21,25 +21,22 @@
#include <stdexcept>
#include <vector>
#include "mynteye/device/config.h"
#include "mynteye/logger.h"
#include "mynteye/util/strings.h"
#include "mynteye/util/times.h"
#include "mynteye/logger.h"
#define IMU_TRACK_PERIOD 25 // ms
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) {
@ -122,15 +119,15 @@ void CheckSpecVersion(const Version *spec_version) {
} // namespace
Channels::Channels(const Model &model, std::shared_ptr<uvc::device> device)
: device_(device),
model_(model),
is_imu_tracking_(false),
imu_track_stop_(false),
imu_sn_(0),
imu_callback_(nullptr) {
Channels::Channels(const std::shared_ptr<uvc::device> &device,
const std::shared_ptr<ChannelsAdapter> &adapter)
: device_(device),
adapter_(adapter),
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();
}
@ -139,6 +136,14 @@ Channels::~Channels() {
StopImuTracking();
}
std::int32_t Channels::GetAccelRangeDefault() {
return adapter_->GetAccelRangeDefault();
}
std::int32_t Channels::GetGyroRangeDefault() {
return adapter_->GetGyroRangeDefault();
}
void Channels::LogControlInfos() const {
for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++) {
LOG(INFO) << it->first << ": min=" << it->second.min
@ -148,34 +153,32 @@ void Channels::LogControlInfos() const {
}
void Channels::UpdateControlInfos() {
auto &&supports = option_supports_map.at(model_);
auto &&supports = adapter_->GetOptionSupports();
for (auto &&option : std::vector<Option>{
Option::GAIN, Option::BRIGHTNESS,
Option::CONTRAST}) {
Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST}) {
if (supports.find(option) != supports.end())
control_infos_[option] = PuControlInfo(option);
}
}
for (auto &&option : std::vector<Option>{
Option::FRAME_RATE, Option::IMU_FREQUENCY,
Option::EXPOSURE_MODE, Option::MAX_GAIN,
Option::MAX_EXPOSURE_TIME, Option::MIN_EXPOSURE_TIME,
Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL,
Option::HDR_MODE, Option::ACCELEROMETER_RANGE,
Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
Option::GYROSCOPE_LOW_PASS_FILTER}) {
if (supports.find(option) != supports.end())
control_infos_[option] = XuControlInfo(option);
}
for (auto &&option : std::vector<Option>{
Option::FRAME_RATE, Option::IMU_FREQUENCY,
Option::EXPOSURE_MODE, Option::MAX_GAIN,
Option::MAX_EXPOSURE_TIME, Option::MIN_EXPOSURE_TIME,
Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL,
Option::HDR_MODE, Option::ACCELEROMETER_RANGE,
Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
Option::GYROSCOPE_LOW_PASS_FILTER}) {
if (supports.find(option) != supports.end())
control_infos_[option] = XuControlInfo(option);
}
if (VLOG_IS_ON(2)) {
for (auto &&it = control_infos_.begin(); it != control_infos_.end();
it++) {
VLOG(2) << it->first << ": min=" << it->second.min
<< ", max=" << it->second.max << ", def=" << it->second.def
<< ", cur=" << GetControlValue(it->first);
}
if (VLOG_IS_ON(2)) {
for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++) {
VLOG(2) << it->first << ": min=" << it->second.min
<< ", max=" << it->second.max << ", def=" << it->second.def
<< ", cur=" << GetControlValue(it->first);
}
}
}
Channels::control_info_t Channels::GetControlInfo(const Option &option) const {
@ -268,25 +271,13 @@ void Channels::SetControlValue(const Option &option, std::int32_t value) {
XuCamCtrlSet(option, value);
} break;
case Option::ACCELEROMETER_RANGE: {
if (model_ == Model::STANDARD) {
if (!in_range() || !in_values({4, 8, 16, 32}))
break;
}
if (model_ == Model::STANDARD2) {
if (!in_range() || !in_values({6, 12, 24, 48}))
break;
}
if (!in_range() || !in_values(adapter_->GetAccelRangeValues()))
break;
XuCamCtrlSet(option, value);
} break;
case Option::GYROSCOPE_RANGE: {
if (model_ == Model::STANDARD) {
if (!in_range() || !in_values({500, 1000, 2000, 4000}))
break;
}
if (model_ == Model::STANDARD2) {
if (!in_range() || !in_values({250, 500, 1000, 2000, 4000}))
break;
}
if (!in_range() || !in_values(adapter_->GetGyroRangeValues()))
break;
XuCamCtrlSet(option, value);
} break;
case Option::ACCELEROMETER_LOW_PASS_FILTER: {
@ -349,17 +340,13 @@ 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(imu_res_version_);
static ImuResPacket res_packet;
req_packet.serial_number = imu_sn_;
if (!XuImuWrite(req_packet)) {
@ -1094,7 +1081,7 @@ bool Channels::XuImuRead(ImuResPacket *res) const {
static std::uint8_t data[2000]{};
// std::fill(data, data + 2000, 0); // reset
if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
res->from_data(data);
adapter_->GetImuResPacket(data, res);
if (res->header != 0x5B) {
LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"

33
src/mynteye/device/channels.h
View File

@ -17,7 +17,9 @@
#include <map>
#include <memory>
#include <set>
#include <thread>
#include <vector>
#include "mynteye/mynteye.h"
#include "mynteye/types.h"
@ -34,6 +36,8 @@ struct xu;
} // namespace uvc
class ChannelsAdapter;
class MYNTEYE_API Channels {
public:
typedef enum Channel {
@ -71,10 +75,13 @@ class MYNTEYE_API Channels {
using img_params_t = std::map<Resolution, device::img_params_t>;
using imu_params_t = device::imu_params_t;
explicit Channels(
const Model &model, std::shared_ptr<uvc::device> device);
Channels(const std::shared_ptr<uvc::device> &device,
const std::shared_ptr<ChannelsAdapter> &adapter);
~Channels();
std::int32_t GetAccelRangeDefault();
std::int32_t GetGyroRangeDefault();
void LogControlInfos() const;
void UpdateControlInfos();
control_info_t GetControlInfo(const Option &option) const;
@ -84,8 +91,6 @@ class MYNTEYE_API Channels {
bool RunControlAction(const Option &option) const;
std::uint8_t GetImuResVersion();
void SetImuCallback(imu_callback_t callback);
void DoImuTrack();
@ -132,11 +137,8 @@ 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<uvc::device> device_;
std::shared_ptr<ChannelsAdapter> adapter_;
std::map<Option, control_info_t> control_infos_;
@ -148,6 +150,21 @@ class MYNTEYE_API Channels {
imu_callback_t imu_callback_;
};
class ChannelsAdapter {
public:
virtual ~ChannelsAdapter() {}
virtual std::set<Option> GetOptionSupports() = 0;
virtual std::int32_t GetAccelRangeDefault() = 0;
virtual std::vector<std::int32_t> GetAccelRangeValues() = 0;
virtual std::int32_t GetGyroRangeDefault() = 0;
virtual std::vector<std::int32_t> GetGyroRangeValues() = 0;
virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_CHANNELS_H_

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

@ -79,14 +79,17 @@ bool CheckSupports(
} // namespace
Device::Device(const Model &model, std::shared_ptr<uvc::device> device)
: video_streaming_(false),
motion_tracking_(false),
model_(model),
device_(device),
streams_(nullptr),
channels_(std::make_shared<Channels>(model_, device)),
motions_(std::make_shared<Motions>(channels_)) {
Device::Device(const Model &model,
const std::shared_ptr<uvc::device> &device,
const std::shared_ptr<StreamsAdapter> &streams_adapter,
const std::shared_ptr<ChannelsAdapter> &channels_adapter)
: video_streaming_(false),
motion_tracking_(false),
model_(model),
device_(device),
streams_(std::make_shared<Streams>(streams_adapter)),
channels_(std::make_shared<Channels>(device_, channels_adapter)),
motions_(std::make_shared<Motions>(channels_)) {
VLOG(2) << __func__;
ReadAllInfos();
}
@ -471,8 +474,6 @@ void Device::StartVideoStreaming() {
return;
}
streams_ = std::make_shared<Streams>(CreateStreamsAdapter());
// if stream capabilities are supported with subdevices of device_
/*
Capabilities stream_capabilities[] = {

4
src/mynteye/device/motions.cc
View File

@ -36,11 +36,11 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
if (motion_callback_) {
accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE);
if (accel_range == -1)
accel_range = (channels_->GetImuResVersion() == 1) ? 8 : 12;
accel_range = channels_->GetAccelRangeDefault();
gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE);
if (gyro_range == -1)
gyro_range = 1000;
gyro_range = channels_->GetGyroRangeDefault();
channels_->SetImuCallback([this](const ImuPacket &packet) {
if (!motion_callback_ && !motion_datas_enabled_) {

129
src/mynteye/device/standard/channels_adapter_s.cc Normal file
View File

@ -0,0 +1,129 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mynteye/device/standard/channels_adapter_s.h"
#include "mynteye/device/config.h"
#include "mynteye/logger.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
#pragma pack(push, 1)
struct ImuData {
std::int16_t offset;
std::uint16_t frame_id;
std::int16_t accel[3];
std::int16_t temperature;
std::int16_t gyro[3];
ImuData() = default;
explicit ImuData(const std::uint8_t *data) {
from_data(data);
}
void from_data(const 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);
}
};
#pragma pack(pop)
void unpack_imu_segment(const ImuData &imu, const std::uint32_t &timestamp,
ImuSegment *seg) {
seg->frame_id = static_cast<uint32_t>(imu.frame_id);
seg->timestamp = static_cast<uint64_t>(timestamp + imu.offset) * 10;
seg->flag = 0;
seg->temperature = imu.temperature;
seg->accel[0] = imu.accel[0];
seg->accel[1] = imu.accel[1];
seg->accel[2] = imu.accel[2];
seg->gyro[0] = imu.gyro[0];
seg->gyro[1] = imu.gyro[1];
seg->gyro[2] = imu.gyro[2];
}
void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
pkg->serial_number =
(*(data) << 24) | (*(data + 1) << 16) |
(*(data + 2) << 8) | *(data + 3);
std::uint32_t timestamp =
(*(data + 4) << 24) | (*(data + 5) << 16)|
(*(data + 6) << 8) | *(data + 7);
pkg->count = *(data + 8);
std::size_t data_n = sizeof(ImuData); // 18
for (std::size_t i = 0; i < pkg->count; i++) {
ImuSegment seg;
unpack_imu_segment(ImuData(data + 9 + (data_n * i)), timestamp, &seg);
pkg->segments.push_back(seg);
}
}
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
res->header = *data;
res->state = *(data + 1);
res->size = (*(data + 2) << 8) | *(data + 3);
std::size_t data_n = sizeof(ImuData); // 18
for (std::size_t i = 4; i < res->size;) {
ImuPacket packet;
unpack_imu_packet(data + i, &packet);
res->packets.push_back(packet);
i += 9 + (packet.count * data_n);
}
res->checksum = *(data + 4 + res->size);
}
} // namespace
StandardChannelsAdapter::StandardChannelsAdapter() {
}
StandardChannelsAdapter::~StandardChannelsAdapter() {
}
std::set<Option> StandardChannelsAdapter::GetOptionSupports() {
return option_supports_map.at(Model::STANDARD);
}
std::int32_t StandardChannelsAdapter::GetAccelRangeDefault() {
return 8;
}
std::vector<std::int32_t> StandardChannelsAdapter::GetAccelRangeValues() {
return {4, 8, 16, 32};
}
std::int32_t StandardChannelsAdapter::GetGyroRangeDefault() {
return 1000;
}
std::vector<std::int32_t> StandardChannelsAdapter::GetGyroRangeValues() {
return {500, 1000, 2000, 4000};
}
void StandardChannelsAdapter::GetImuResPacket(
const std::uint8_t *data, ImuResPacket *res) {
unpack_imu_res_packet(data, res);
}
MYNTEYE_END_NAMESPACE

44
src/mynteye/device/standard/channels_adapter_s.h Normal file
View File

@ -0,0 +1,44 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_STANDARD_CHANNELS_ADAPTER_S_H_
#define MYNTEYE_DEVICE_STANDARD_CHANNELS_ADAPTER_S_H_
#pragma once
#include <cstdint>
#include <set>
#include <vector>
#include "mynteye/device/channels.h"
MYNTEYE_BEGIN_NAMESPACE
class StandardChannelsAdapter : public ChannelsAdapter {
public:
StandardChannelsAdapter();
virtual ~StandardChannelsAdapter();
std::set<Option> GetOptionSupports() override;
std::int32_t GetAccelRangeDefault() override;
std::vector<std::int32_t> GetAccelRangeValues() override;
std::int32_t GetGyroRangeDefault() override;
std::vector<std::int32_t> GetGyroRangeValues() override;
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD_CHANNELS_ADAPTER_S_H_

9
src/mynteye/device/standard/device_s.cc
View File

@ -15,12 +15,15 @@
#include "mynteye/logger.h"
#include "mynteye/device/motions.h"
#include "mynteye/device/standard/channels_adapter_s.h"
#include "mynteye/device/standard/streams_adapter_s.h"
MYNTEYE_BEGIN_NAMESPACE
StandardDevice::StandardDevice(std::shared_ptr<uvc::device> device)
: Device(Model::STANDARD, device) {
: Device(Model::STANDARD, device,
std::make_shared<StandardStreamsAdapter>(),
std::make_shared<StandardChannelsAdapter>()) {
VLOG(2) << __func__;
}
@ -32,10 +35,6 @@ Capabilities StandardDevice::GetKeyStreamCapability() const {
return Capabilities::STEREO;
}
std::shared_ptr<StreamsAdapter> StandardDevice::CreateStreamsAdapter() const {
return std::make_shared<StandardStreamsAdapter>();
}
void StandardDevice::StartVideoStreaming() {
// TODO(John) Set img framerate, imu frequency according to stream request
Device::StartVideoStreaming();

1
src/mynteye/device/standard/device_s.h
View File

@ -28,7 +28,6 @@ class StandardDevice : public Device {
virtual ~StandardDevice();
Capabilities GetKeyStreamCapability() const override;
std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const override;
void StartVideoStreaming() override;

125
src/mynteye/device/standard2/channels_adapter_s2.cc Normal file
View File

@ -0,0 +1,125 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "mynteye/device/standard2/channels_adapter_s2.h"
#include "mynteye/device/config.h"
#include "mynteye/logger.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
#pragma pack(push, 1)
struct ImuData {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
std::int16_t temperature;
std::int16_t accel_or_gyro[3];
ImuData() = default;
explicit ImuData(const std::uint8_t *data) {
from_data(data);
}
void from_data(const std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
frame_id = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
*(data + 3);
timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
(*(data + 6) << 8) | *(data + 7);
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
flag = *(data + 12);
temperature = (*(data + 13) << 8) | *(data + 14);
accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
accel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
accel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
}
};
#pragma pack(pop)
void unpack_imu_segment(const ImuData &imu, ImuSegment *seg) {
seg->frame_id = imu.frame_id;
seg->timestamp = imu.timestamp;
seg->flag = imu.flag;
seg->temperature = imu.temperature;
seg->accel[0] = (seg->flag == 1) ? imu.accel_or_gyro[0] : 0;
seg->accel[1] = (seg->flag == 1) ? imu.accel_or_gyro[1] : 0;
seg->accel[2] = (seg->flag == 1) ? imu.accel_or_gyro[2] : 0;
seg->gyro[0] = (seg->flag == 2) ? imu.accel_or_gyro[0] : 0;
seg->gyro[1] = (seg->flag == 2) ? imu.accel_or_gyro[1] : 0;
seg->gyro[2] = (seg->flag == 2) ? imu.accel_or_gyro[2] : 0;
}
void unpack_imu_packet(const std::uint8_t *data, ImuPacket *pkg) {
std::size_t data_n = sizeof(ImuData); // 21
for (std::size_t i = 0; i < pkg->count; i++) {
ImuSegment seg;
unpack_imu_segment(ImuData(data + data_n * i), &seg);
pkg->segments.push_back(seg);
}
pkg->serial_number = pkg->segments.back().frame_id;
}
void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
res->header = *data;
res->state = *(data + 1);
res->size = (*(data + 2) << 8) | *(data + 3);
std::size_t data_n = sizeof(ImuData); // 21
ImuPacket packet;
packet.count = res->size / data_n;
unpack_imu_packet(data + 4, &packet);
res->packets.push_back(packet);
res->checksum = *(data + 4 + res->size);
}
} // namespace
Standard2ChannelsAdapter::Standard2ChannelsAdapter() {
}
Standard2ChannelsAdapter::~Standard2ChannelsAdapter() {
}
std::set<Option> Standard2ChannelsAdapter::GetOptionSupports() {
return option_supports_map.at(Model::STANDARD2);
}
std::int32_t Standard2ChannelsAdapter::GetAccelRangeDefault() {
return 12;
}
std::vector<std::int32_t> Standard2ChannelsAdapter::GetAccelRangeValues() {
return {6, 12, 24, 48};
}
std::int32_t Standard2ChannelsAdapter::GetGyroRangeDefault() {
return 1000;
}
std::vector<std::int32_t> Standard2ChannelsAdapter::GetGyroRangeValues() {
return {250, 500, 1000, 2000, 4000};
}
void Standard2ChannelsAdapter::GetImuResPacket(
const std::uint8_t *data, ImuResPacket *res) {
unpack_imu_res_packet(data, res);
}
MYNTEYE_END_NAMESPACE

44
src/mynteye/device/standard2/channels_adapter_s2.h Normal file
View File

@ -0,0 +1,44 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S2_H_
#define MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S2_H_
#pragma once
#include <cstdint>
#include <set>
#include <vector>
#include "mynteye/device/channels.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard2ChannelsAdapter : public ChannelsAdapter {
public:
Standard2ChannelsAdapter();
virtual ~Standard2ChannelsAdapter();
std::set<Option> GetOptionSupports() override;
std::int32_t GetAccelRangeDefault() override;
std::vector<std::int32_t> GetAccelRangeValues() override;
std::int32_t GetGyroRangeDefault() override;
std::vector<std::int32_t> GetGyroRangeValues() override;
void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S2_H_

9
src/mynteye/device/standard2/device_s2.cc
View File

@ -15,12 +15,15 @@
#include "mynteye/logger.h"
#include "mynteye/device/motions.h"
#include "mynteye/device/standard2/channels_adapter_s2.h"
#include "mynteye/device/standard2/streams_adapter_s2.h"
MYNTEYE_BEGIN_NAMESPACE
Standard2Device::Standard2Device(std::shared_ptr<uvc::device> device)
: Device(Model::STANDARD2, device) {
: Device(Model::STANDARD2, device,
std::make_shared<Standard2StreamsAdapter>(),
std::make_shared<Standard2ChannelsAdapter>()) {
VLOG(2) << __func__;
}
@ -32,10 +35,6 @@ Capabilities Standard2Device::GetKeyStreamCapability() const {
return Capabilities::STEREO_COLOR;
}
std::shared_ptr<StreamsAdapter> Standard2Device::CreateStreamsAdapter() const {
return std::make_shared<Standard2StreamsAdapter>();
}
void Standard2Device::OnStereoStreamUpdate() {
if (motion_tracking_) {
auto &&motions = this->motions();

1
src/mynteye/device/standard2/device_s2.h
View File

@ -28,7 +28,6 @@ class Standard2Device : public Device {
virtual ~Standard2Device();
Capabilities GetKeyStreamCapability() const override;
std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const override;
void OnStereoStreamUpdate() override;
};

153
src/mynteye/device/types.h
View File

@ -162,74 +162,6 @@ struct ImuReqPacket {
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu group for standard 1.
*/
#pragma pack(push, 1)
struct ImuGroupS1 {
std::int16_t offset;
std::uint16_t frame_id;
std::int16_t accel[3];
std::int16_t temperature;
std::int16_t gyro[3];
ImuGroupS1() = default;
explicit ImuGroupS1(std::uint8_t *data) {
from_data(data);
}
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);
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu group for standard 2.
*/
#pragma pack(push, 1)
struct ImuGroupS2 {
std::uint32_t frame_id;
std::uint64_t timestamp;
std::uint8_t flag;
std::int16_t temperature;
std::int16_t accel_or_gyro[3];
ImuGroupS2() = default;
explicit ImuGroupS2(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
frame_id = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
*(data + 3);
timestamp_h = (*(data + 4) << 24) | (*(data + 5) << 16) |
(*(data + 6) << 8) | *(data + 7);
timestamp_l = (*(data + 8) << 24) | (*(data + 9) << 16) |
(*(data + 10) << 8) | *(data + 11);
timestamp = (static_cast<std::uint64_t>(timestamp_h) << 32) | timestamp_l;
flag = *(data + 12);
temperature = (*(data + 13) << 8) | *(data + 14);
accel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
accel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
accel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
}
};
#pragma pack(pop)
/**
* @ingroup datatypes
* Imu segment.
@ -242,32 +174,6 @@ struct ImuSegment {
std::int16_t temperature;
std::int16_t accel[3];
std::int16_t gyro[3];
ImuSegment() = default;
explicit ImuSegment(ImuGroupS2 group)
: frame_id(group.frame_id), timestamp(group.timestamp),
flag(group.flag), temperature(group.temperature) {
accel[0] = (flag == 1) ? group.accel_or_gyro[0] : 0;
accel[1] = (flag == 1) ? group.accel_or_gyro[1] : 0;
accel[2] = (flag == 1) ? group.accel_or_gyro[2] : 0;
gyro[0] = (flag == 2) ? group.accel_or_gyro[0] : 0;
gyro[1] = (flag == 2) ? group.accel_or_gyro[1] : 0;
gyro[2] = (flag == 2) ? group.accel_or_gyro[2] : 0;
}
ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) {
frame_id = static_cast<uint32_t> (group.frame_id);
this->timestamp = static_cast<uint64_t> (timestamp + group.offset) * 10;
flag = 3;
temperature = group.temperature;
accel[0] = group.accel[0];
accel[1] = group.accel[1];
accel[2] = group.accel[2];
gyro[0] = group.gyro[0];
gyro[1] = group.gyro[1];
gyro[2] = group.gyro[2];
}
};
#pragma pack(pop)
@ -281,37 +187,6 @@ struct ImuPacket {
std::uint8_t count;
std::uint32_t serial_number;
std::vector<ImuSegment> segments;
ImuPacket() = default;
explicit ImuPacket(
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 (version == 1) {
serial_number =
(*(data) << 24) | (*(data + 1) << 16) |
(*(data + 2) << 8) | *(data + 3);
int timestamp =
(*(data + 4) << 24) | (*(data + 5) << 16)|
(*(data + 6) << 8) | *(data + 7);
count = *(data + 8);
std::size_t seg_n = sizeof(ImuGroupS1); // 18
for (std::size_t i = 0; i < count; i++) {
ImuGroupS1 group(data + 9 + (seg_n * i));
segments.push_back(ImuSegment(timestamp, group));
}
} 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);
segments.push_back(ImuSegment(group));
}
serial_number = segments.back().frame_id;
}
}
};
#pragma pack(pop)
@ -327,34 +202,6 @@ struct ImuResPacket {
std::uint16_t size;
std::vector<ImuPacket> packets;
std::uint8_t checksum;
ImuResPacket() = default;
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 (version == 1) {
std::size_t seg_n = sizeof(ImuGroupS1); // 18
for (std::size_t i = 4; i < size;) {
ImuPacket packet(version, 0, data + i);
packets.push_back(packet);
i += 9 + (packet.count * seg_n);
}
checksum = *(data + 4 + size);
}
if (version == 2) {
std::size_t seg_n = sizeof(ImuGroupS2); // 21
std::uint8_t seg_count = size / seg_n;
ImuPacket packet(version, seg_count, data + 4);
packets.push_back(packet);
// packet(2);
checksum = *(data + 4 + size);
}
}
};
#pragma pack(pop)