refactor(device): merge device_s210a to device_s2

This commit is contained in:
John Zhao 2019-03-02 11:47:20 +08:00
parent a5b337b50f
commit 70e1e9e32f
14 changed files with 81 additions and 494 deletions

View File

@ -199,9 +199,6 @@ set(MYNTEYE_SRCS
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/standard2/channels_adapter_s210a.cc
src/mynteye/device/standard2/device_s210a.cc
src/mynteye/device/standard2/streams_adapter_s210a.cc
src/mynteye/device/streams.cc
src/mynteye/device/types.cc
src/mynteye/device/utils.cc

View File

@ -26,7 +26,6 @@
#include "mynteye/device/motions.h"
#include "mynteye/device/standard/device_s.h"
#include "mynteye/device/standard2/device_s2.h"
#include "mynteye/device/standard2/device_s210a.h"
#include "mynteye/device/streams.h"
#include "mynteye/device/types.h"
#include "mynteye/util/strings.h"
@ -104,7 +103,6 @@ std::shared_ptr<Device> Device::Create(
if (name == "MYNTEYE") {
return std::make_shared<StandardDevice>(device);
} else if (strings::starts_with(name, "MYNT-EYE-")) {
// TODO(JohnZhao): Create different device by name, such as MYNT-EYE-S1000
std::string model_s = name.substr(9, 5);
VLOG(2) << "MYNE EYE Model: " << model_s;
DeviceModel model(model_s);
@ -113,9 +111,9 @@ std::shared_ptr<Device> Device::Create(
return std::make_shared<StandardDevice>(device);
} else if (model.generation == '2') {
if (model.custom_code == '0') {
return std::make_shared<Standard2Device>(device);
return std::make_shared<Standard2Device>(Model::STANDARD2, device);
} else if (model.custom_code == 'A') {
return std::make_shared<Standard210aDevice>(device);
return std::make_shared<Standard2Device>(Model::STANDARD210A, device);
} else {
LOG(FATAL) << "No such custom code now";
}

View File

@ -90,8 +90,8 @@ void unpack_imu_res_packet(const std::uint8_t *data, ImuResPacket *res) {
} // namespace
Standard2ChannelsAdapter::Standard2ChannelsAdapter()
: ChannelsAdapter(Model::STANDARD2) {
Standard2ChannelsAdapter::Standard2ChannelsAdapter(const Model &model)
: ChannelsAdapter(model) {
}
Standard2ChannelsAdapter::~Standard2ChannelsAdapter() {

View File

@ -25,7 +25,7 @@ MYNTEYE_BEGIN_NAMESPACE
class Standard2ChannelsAdapter : public ChannelsAdapter {
public:
Standard2ChannelsAdapter();
explicit Standard2ChannelsAdapter(const Model &model);
virtual ~Standard2ChannelsAdapter();
std::int32_t GetAccelRangeDefault() override;

View File

@ -1,121 +0,0 @@
// 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_s210a.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
Standard210aChannelsAdapter::Standard210aChannelsAdapter()
: ChannelsAdapter(Model::STANDARD210A) {
}
Standard210aChannelsAdapter::~Standard210aChannelsAdapter() {
}
std::int32_t Standard210aChannelsAdapter::GetAccelRangeDefault() {
return 12;
}
std::vector<std::int32_t> Standard210aChannelsAdapter::GetAccelRangeValues() {
return {6, 12, 24, 48};
}
std::int32_t Standard210aChannelsAdapter::GetGyroRangeDefault() {
return 1000;
}
std::vector<std::int32_t> Standard210aChannelsAdapter::GetGyroRangeValues() {
return {250, 500, 1000, 2000, 4000};
}
void Standard210aChannelsAdapter::GetImuResPacket(
const std::uint8_t *data, ImuResPacket *res) {
unpack_imu_res_packet(data, res);
}
MYNTEYE_END_NAMESPACE

View File

@ -1,42 +0,0 @@
// 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_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S210A_H_
#pragma once
#include <cstdint>
#include <set>
#include <vector>
#include "mynteye/device/channel/channels.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aChannelsAdapter : public ChannelsAdapter {
public:
Standard210aChannelsAdapter();
virtual ~Standard210aChannelsAdapter();
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_S210A_H_

View File

@ -20,11 +20,13 @@
MYNTEYE_BEGIN_NAMESPACE
Standard2Device::Standard2Device(std::shared_ptr<uvc::device> device)
: Device(Model::STANDARD2, device,
std::make_shared<Standard2StreamsAdapter>(),
std::make_shared<Standard2ChannelsAdapter>()) {
Standard2Device::Standard2Device(const Model &model,
std::shared_ptr<uvc::device> device)
: Device(model, device,
std::make_shared<Standard2StreamsAdapter>(model),
std::make_shared<Standard2ChannelsAdapter>(model)) {
VLOG(2) << __func__;
CHECK(model == Model::STANDARD2 || model == Model::STANDARD210A);
}
Standard2Device::~Standard2Device() {

View File

@ -24,7 +24,7 @@ MYNTEYE_BEGIN_NAMESPACE
class Standard2Device : public Device {
public:
explicit Standard2Device(std::shared_ptr<uvc::device> device);
Standard2Device(const Model &model, std::shared_ptr<uvc::device> device);
virtual ~Standard2Device();
Capabilities GetKeyStreamCapability() const override;

View File

@ -1,45 +0,0 @@
// 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/device_s210a.h"
#include "mynteye/logger.h"
#include "mynteye/device/motions.h"
#include "mynteye/device/standard2/channels_adapter_s210a.h"
#include "mynteye/device/standard2/streams_adapter_s210a.h"
MYNTEYE_BEGIN_NAMESPACE
Standard210aDevice::Standard210aDevice(std::shared_ptr<uvc::device> device)
: Device(Model::STANDARD210A, device,
std::make_shared<Standard210aStreamsAdapter>(),
std::make_shared<Standard210aChannelsAdapter>()) {
VLOG(2) << __func__;
}
Standard210aDevice::~Standard210aDevice() {
VLOG(2) << __func__;
}
Capabilities Standard210aDevice::GetKeyStreamCapability() const {
return Capabilities::STEREO_COLOR;
}
void Standard210aDevice::OnStereoStreamUpdate() {
if (motion_tracking_) {
auto &&motions = this->motions();
motions->DoMotionTrack();
}
}
MYNTEYE_END_NAMESPACE

View File

@ -1,37 +0,0 @@
// 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_DEVICE_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_DEVICE_S210A_H_
#pragma once
#include <memory>
#include <vector>
#include "mynteye/device/device.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aDevice : public Device {
public:
explicit Standard210aDevice(std::shared_ptr<uvc::device> device);
virtual ~Standard210aDevice();
Capabilities GetKeyStreamCapability() const override;
void OnStereoStreamUpdate() override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_DEVICE_S210A_H_

View File

@ -143,7 +143,58 @@ bool unpack_stereo_img_data(
} // namespace
Standard2StreamsAdapter::Standard2StreamsAdapter() {
namespace s210a {
// image pixels
bool unpack_left_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t h = frame->height();
for (std::size_t i = 0; i < h; i++) {
for (std::size_t j = 0; j < w; j++) {
frame->data()[(i * w + j) * n] =
*(data_new + (2 * i * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + (2 * i * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + (2 * i * w + j) * n);
}
}
return true;
}
bool unpack_right_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t h = frame->height();
for (std::size_t i = 0; i < h; i++) {
for (std::size_t j = 0; j < w; j++) {
frame->data()[(i * w + j) * n] =
*(data_new + ((2 * i + 1) * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + ((2 * i + 1) * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + ((2 * i + 1) * w + j) * n);
}
}
return true;
}
} // namespace s210a
Standard2StreamsAdapter::Standard2StreamsAdapter(const Model &model)
: model_(model) {
}
Standard2StreamsAdapter::~Standard2StreamsAdapter() {
@ -167,10 +218,19 @@ Standard2StreamsAdapter::GetUnpackImgDataMap() {
std::map<Stream, Streams::unpack_img_pixels_t>
Standard2StreamsAdapter::GetUnpackImgPixelsMap() {
switch (model_) {
case Model::STANDARD210A:
return {
{Stream::LEFT, s210a::unpack_left_img_pixels},
{Stream::RIGHT, s210a::unpack_right_img_pixels}
};
case Model::STANDARD2:
default:
return {
{Stream::LEFT, unpack_left_img_pixels},
{Stream::RIGHT, unpack_right_img_pixels}
};
}
}
MYNTEYE_END_NAMESPACE

View File

@ -25,7 +25,7 @@ MYNTEYE_BEGIN_NAMESPACE
class Standard2StreamsAdapter : public StreamsAdapter {
public:
Standard2StreamsAdapter();
explicit Standard2StreamsAdapter(const Model &model);
virtual ~Standard2StreamsAdapter();
std::vector<Stream> GetKeyStreams() override;
@ -35,6 +35,9 @@ class Standard2StreamsAdapter : public StreamsAdapter {
GetUnpackImgDataMap() override;
std::map<Stream, Streams::unpack_img_pixels_t>
GetUnpackImgPixelsMap() override;
private:
Model model_;
};
MYNTEYE_END_NAMESPACE

View File

@ -1,186 +0,0 @@
// 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/streams_adapter_s210a.h"
#include <iomanip>
#include "mynteye/logger.h"
#include "mynteye/device/types.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
// image info
#pragma pack(push, 1)
struct ImagePacket {
std::uint8_t header;
std::uint8_t size;
std::uint16_t frame_id;
std::uint64_t timestamp;
std::uint16_t exposure_time;
std::uint8_t checksum;
ImagePacket() = default;
explicit ImagePacket(std::uint8_t *data) {
from_data(data);
}
void from_data(std::uint8_t *data) {
std::uint32_t timestamp_l;
std::uint32_t timestamp_h;
header = *data;
size = *(data + 1);
frame_id = (*(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;
exposure_time = (*(data + 12) << 8) | *(data + 13);
checksum = *(data + 14);
}
};
#pragma pack(pop)
// image pixels
bool unpack_left_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t h = frame->height();
for (std::size_t i = 0; i < h; i++) {
for (std::size_t j = 0; j < w; j++) {
frame->data()[(i * w + j) * n] =
*(data_new + (2 * i * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + (2 * i * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + (2 * i * w + j) * n);
}
}
return true;
}
bool unpack_right_img_pixels(
const void *data, const StreamRequest &request, Streams::frame_t *frame) {
CHECK_NOTNULL(frame);
CHECK_EQ(request.format, Format::BGR888);
CHECK_EQ(frame->format(), Format::BGR888);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t n = 3;
std::size_t w = frame->width();
std::size_t h = frame->height();
for (std::size_t i = 0; i < h; i++) {
for (std::size_t j = 0; j < w; j++) {
frame->data()[(i * w + j) * n] =
*(data_new + ((2 * i + 1) * w + j) * n + 2);
frame->data()[(i * w + j) * n + 1] =
*(data_new + ((2 * i + 1) * w + j) * n + 1);
frame->data()[(i * w + j) * n + 2] =
*(data_new + ((2 * i + 1) * w + j) * n);
}
}
return true;
}
bool unpack_stereo_img_data(
const void *data, const StreamRequest &request, ImgData *img) {
CHECK_NOTNULL(img);
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
std::size_t data_n =
request.width * request.height * bytes_per_pixel(request.format);
auto data_end = data_new + data_n;
std::size_t packet_n = sizeof(ImagePacket);
std::vector<std::uint8_t> packet(packet_n);
std::reverse_copy(data_end - packet_n, data_end, packet.begin());
ImagePacket img_packet(packet.data());
// LOG(INFO) << "ImagePacket: header=0x" << std::hex <<
// static_cast<int>(img_packet.header)
// << ", size=0x" << std::hex << static_cast<int>(img_packet.size)
// << ", frame_id="<< std::dec << img_packet.frame_id
// << ", timestamp="<< std::dec << img_packet.timestamp
// << ", exposure_time="<< std::dec << img_packet.exposure_time
// << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
if (img_packet.header != 0x3B) {
VLOG(2) << "Image packet header must be 0x3B, but 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(img_packet.header) << " now";
return false;
}
std::uint8_t checksum = 0;
for (std::size_t i = 2, n = packet_n - 2; i <= n; i++) { // content: [2,9]
checksum = (checksum ^ packet[i]);
}
/*
if (img_packet.checksum != checksum) {
VLOG(2) << "Image packet checksum should be 0x" << std::hex
<< std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(img_packet.checksum) << ", but 0x"
<< std::setw(2) << std::setfill('0') << static_cast<int>(checksum)
<< " now";
return false;
}
*/
img->frame_id = img_packet.frame_id;
img->timestamp = img_packet.timestamp;
img->exposure_time = img_packet.exposure_time;
return true;
}
} // namespace
Standard210aStreamsAdapter::Standard210aStreamsAdapter() {
}
Standard210aStreamsAdapter::~Standard210aStreamsAdapter() {
}
std::vector<Stream> Standard210aStreamsAdapter::GetKeyStreams() {
return {Stream::LEFT, Stream::RIGHT};
}
std::vector<Capabilities> Standard210aStreamsAdapter::GetStreamCapabilities() {
return {Capabilities::STEREO_COLOR};
}
std::map<Stream, Streams::unpack_img_data_t>
Standard210aStreamsAdapter::GetUnpackImgDataMap() {
return {
{Stream::LEFT, unpack_stereo_img_data},
{Stream::RIGHT, unpack_stereo_img_data}
};
}
std::map<Stream, Streams::unpack_img_pixels_t>
Standard210aStreamsAdapter::GetUnpackImgPixelsMap() {
return {
{Stream::LEFT, unpack_left_img_pixels},
{Stream::RIGHT, unpack_right_img_pixels}
};
}
MYNTEYE_END_NAMESPACE

View File

@ -1,42 +0,0 @@
// 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_STREAMS_ADAPTER_S210A_H_
#define MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S210A_H_
#pragma once
#include <map>
#include <memory>
#include <vector>
#include "mynteye/device/streams.h"
MYNTEYE_BEGIN_NAMESPACE
class Standard210aStreamsAdapter : public StreamsAdapter {
public:
Standard210aStreamsAdapter();
virtual ~Standard210aStreamsAdapter();
std::vector<Stream> GetKeyStreams() override;
std::vector<Capabilities> GetStreamCapabilities() override;
std::map<Stream, Streams::unpack_img_data_t>
GetUnpackImgDataMap() override;
std::map<Stream, Streams::unpack_img_pixels_t>
GetUnpackImgPixelsMap() override;
};
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S210A_H_