feat(root): add s210a
This commit is contained in:
@@ -17,12 +17,14 @@ MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
const std::map<Model, StreamSupports> stream_supports_map = {
|
||||
{Model::STANDARD, {Stream::LEFT, Stream::RIGHT}},
|
||||
{Model::STANDARD2, {Stream::LEFT, Stream::RIGHT}}
|
||||
{Model::STANDARD2, {Stream::LEFT, Stream::RIGHT}},
|
||||
{Model::STANDARD210A, {Stream::LEFT, Stream::RIGHT}}
|
||||
};
|
||||
|
||||
const std::map<Model, CapabilitiesSupports> capabilities_supports_map = {
|
||||
{Model::STANDARD, {Capabilities::STEREO, Capabilities::IMU}},
|
||||
{Model::STANDARD2, {Capabilities::STEREO_COLOR, Capabilities::IMU}}
|
||||
{Model::STANDARD2, {Capabilities::STEREO_COLOR, Capabilities::IMU}},
|
||||
{Model::STANDARD210A, {Capabilities::STEREO_COLOR, Capabilities::IMU}}
|
||||
};
|
||||
|
||||
const std::map<Model, OptionSupports> option_supports_map = {
|
||||
@@ -38,6 +40,14 @@ const std::map<Model, OptionSupports> option_supports_map = {
|
||||
Option::ERASE_CHIP}
|
||||
},
|
||||
{Model::STANDARD2, {
|
||||
Option::BRIGHTNESS,
|
||||
Option::EXPOSURE_MODE, Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
|
||||
Option::IR_CONTROL, Option::MIN_EXPOSURE_TIME,
|
||||
Option::DESIRED_BRIGHTNESS, Option::ACCELEROMETER_RANGE,
|
||||
Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
|
||||
Option::GYROSCOPE_LOW_PASS_FILTER, Option::ERASE_CHIP}
|
||||
},
|
||||
{Model::STANDARD210A, {
|
||||
Option::BRIGHTNESS,
|
||||
Option::EXPOSURE_MODE, Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
|
||||
Option::MIN_EXPOSURE_TIME, Option::DESIRED_BRIGHTNESS,
|
||||
@@ -55,6 +65,17 @@ stream_requests_map = {
|
||||
}}
|
||||
},
|
||||
{Model::STANDARD2,
|
||||
{{Capabilities::STEREO_COLOR, {
|
||||
{1280, 400, Format::YUYV, 10},
|
||||
{1280, 400, Format::YUYV, 20},
|
||||
{1280, 400, Format::YUYV, 30},
|
||||
{1280, 400, Format::YUYV, 60},
|
||||
{2560, 800, Format::YUYV, 10},
|
||||
{2560, 800, Format::YUYV, 20},
|
||||
{2560, 800, Format::YUYV, 30}}
|
||||
}}
|
||||
},
|
||||
{Model::STANDARD210A,
|
||||
{{Capabilities::STEREO_COLOR, {
|
||||
{1280, 400, Format::BGR888, 10},
|
||||
{1280, 400, Format::BGR888, 20},
|
||||
|
||||
@@ -26,6 +26,7 @@
|
||||
#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"
|
||||
@@ -108,13 +109,18 @@ std::shared_ptr<Device> Device::Create(
|
||||
VLOG(2) << "MYNE EYE Model: " << model_s;
|
||||
DeviceModel model(model_s);
|
||||
if (model.type == 'S') {
|
||||
switch (model.generation) {
|
||||
case '1':
|
||||
return std::make_shared<StandardDevice>(device);
|
||||
case '2':
|
||||
if (model.generation == '1') {
|
||||
return std::make_shared<StandardDevice>(device);
|
||||
} else if (model.generation == '2') {
|
||||
if (model.custom_code == '0') {
|
||||
return std::make_shared<Standard2Device>(device);
|
||||
default:
|
||||
LOG(FATAL) << "No such generation now";
|
||||
} else if (model.custom_code == 'A') {
|
||||
return std::make_shared<Standard210aDevice>(device);
|
||||
} else {
|
||||
LOG(FATAL) << "No such custom code now";
|
||||
}
|
||||
} else {
|
||||
LOG(FATAL) << "No such generation now";
|
||||
}
|
||||
} else {
|
||||
LOG(FATAL) << "MYNT EYE model is not supported now";
|
||||
|
||||
173
src/mynteye/device/standard2/channels_adapter_s210a.cc
Normal file
173
src/mynteye/device/standard2/channels_adapter_s210a.cc
Normal file
@@ -0,0 +1,173 @@
|
||||
// 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/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
|
||||
|
||||
Standard210aChannelsAdapter::Standard210aChannelsAdapter() {
|
||||
}
|
||||
|
||||
Standard210aChannelsAdapter::~Standard210aChannelsAdapter() {
|
||||
}
|
||||
|
||||
std::set<Option> Standard210aChannelsAdapter::GetOptionSupports() {
|
||||
return option_supports_map.at(Model::STANDARD210A);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
std::size_t Standard210aChannelsAdapter::GetImgParamsFromData(
|
||||
const std::uint8_t *data, const Version *version,
|
||||
Channels::img_params_t *img_params) {
|
||||
std::size_t i = 0;
|
||||
|
||||
Intrinsics in_left, in_right;
|
||||
Extrinsics ex_right_to_left;
|
||||
|
||||
i += bytes::from_data(&in_left, data + i, version);
|
||||
i += bytes::from_data(&in_right, data + i, version);
|
||||
(*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left};
|
||||
|
||||
i += bytes::from_data(&in_left, data + i, version);
|
||||
i += bytes::from_data(&in_right, data + i, version);
|
||||
(*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left};
|
||||
|
||||
i += bytes::from_data(&ex_right_to_left, data + i, version);
|
||||
(*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left;
|
||||
(*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
std::size_t Standard210aChannelsAdapter::SetImgParamsToData(
|
||||
const Channels::img_params_t *img_params, const Version *version,
|
||||
std::uint8_t *data) {
|
||||
std::size_t i = 3; // skip id, size
|
||||
|
||||
{
|
||||
auto &¶ms = (*img_params).at({1280, 400});
|
||||
i += bytes::to_data(¶ms.in_left, data + i, version);
|
||||
i += bytes::to_data(¶ms.in_right, data + i, version);
|
||||
}
|
||||
{
|
||||
auto &¶ms = (*img_params).at({2560, 800});
|
||||
i += bytes::to_data(¶ms.in_left, data + i, version);
|
||||
i += bytes::to_data(¶ms.in_right, data + i, version);
|
||||
i += bytes::to_data(¶ms.ex_right_to_left, data + i, version);
|
||||
}
|
||||
|
||||
// others
|
||||
std::size_t size = i - 3;
|
||||
data[0] = Channels::FID_IMG_PARAMS;
|
||||
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
|
||||
data[2] = static_cast<std::uint8_t>(size & 0xFF);
|
||||
return size + 3;
|
||||
}
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
51
src/mynteye/device/standard2/channels_adapter_s210a.h
Normal file
51
src/mynteye/device/standard2/channels_adapter_s210a.h
Normal file
@@ -0,0 +1,51 @@
|
||||
// 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/channels.h"
|
||||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
class Standard210aChannelsAdapter : public ChannelsAdapter {
|
||||
public:
|
||||
Standard210aChannelsAdapter();
|
||||
virtual ~Standard210aChannelsAdapter();
|
||||
|
||||
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;
|
||||
|
||||
std::size_t GetImgParamsFromData(
|
||||
const std::uint8_t *data, const Version *version,
|
||||
Channels::img_params_t *img_params) override;
|
||||
std::size_t SetImgParamsToData(
|
||||
const Channels::img_params_t *img_params, const Version *version,
|
||||
std::uint8_t *data) override;
|
||||
};
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
||||
#endif // MYNTEYE_DEVICE_STANDARD2_CHANNELS_ADAPTER_S210A_H_
|
||||
45
src/mynteye/device/standard2/device_s210a.cc
Normal file
45
src/mynteye/device/standard2/device_s210a.cc
Normal file
@@ -0,0 +1,45 @@
|
||||
// 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
|
||||
37
src/mynteye/device/standard2/device_s210a.h
Normal file
37
src/mynteye/device/standard2/device_s210a.h
Normal file
@@ -0,0 +1,37 @@
|
||||
// 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_
|
||||
@@ -61,20 +61,15 @@ struct ImagePacket {
|
||||
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);
|
||||
CHECK_EQ(request.format, Format::YUYV);
|
||||
CHECK_EQ(frame->format(), Format::YUYV);
|
||||
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
|
||||
std::size_t n = 3;
|
||||
std::size_t w = frame->width();
|
||||
std::size_t n = 2;
|
||||
std::size_t w = frame->width() * n;
|
||||
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);
|
||||
frame->data()[i * w + j] = *(data_new + 2 * i * w + j);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
@@ -83,20 +78,15 @@ bool unpack_left_img_pixels(
|
||||
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);
|
||||
CHECK_EQ(request.format, Format::YUYV);
|
||||
CHECK_EQ(frame->format(), Format::YUYV);
|
||||
auto data_new = reinterpret_cast<const std::uint8_t *>(data);
|
||||
std::size_t n = 3;
|
||||
std::size_t w = frame->width();
|
||||
std::size_t n = 2;
|
||||
std::size_t w = frame->width() * n;
|
||||
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);
|
||||
frame->data()[i * w + j] = *(data_new + (2 * i + 1) * w + j);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
186
src/mynteye/device/standard2/streams_adapter_s210a.cc
Normal file
186
src/mynteye/device/standard2/streams_adapter_s210a.cc
Normal file
@@ -0,0 +1,186 @@
|
||||
// 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
|
||||
42
src/mynteye/device/standard2/streams_adapter_s210a.h
Normal file
42
src/mynteye/device/standard2/streams_adapter_s210a.h
Normal file
@@ -0,0 +1,42 @@
|
||||
// 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_
|
||||
@@ -30,6 +30,7 @@ const char *to_string(const Model &value) {
|
||||
switch (value) {
|
||||
CASE(STANDARD)
|
||||
CASE(STANDARD2)
|
||||
CASE(STANDARD210A)
|
||||
default:
|
||||
CHECK(is_valid(value));
|
||||
return "Model::UNKNOWN";
|
||||
|
||||
Reference in New Issue
Block a user