diff --git a/src/mynteye/device/standard/streams_adapter_s.cc b/src/mynteye/device/standard/streams_adapter_s.cc index eeb0fa6..85d7146 100644 --- a/src/mynteye/device/standard/streams_adapter_s.cc +++ b/src/mynteye/device/standard/streams_adapter_s.cc @@ -22,31 +22,33 @@ MYNTEYE_BEGIN_NAMESPACE namespace { -bool unpack_left_img_pixels( - const void *data, const StreamRequest &request, Streams::frame_t *frame) { - CHECK_NOTNULL(frame); - CHECK_EQ(request.format, Format::YUYV); - CHECK_EQ(frame->format(), Format::GREY); - auto data_new = reinterpret_cast(data); - std::size_t n = frame->width() * frame->height(); - for (std::size_t i = 0; i < n; i++) { - frame->data()[i] = *(data_new + (i * 2)); - } - return true; -} +// image info -bool unpack_right_img_pixels( - const void *data, const StreamRequest &request, Streams::frame_t *frame) { - CHECK_NOTNULL(frame); - CHECK_EQ(request.format, Format::YUYV); - CHECK_EQ(frame->format(), Format::GREY); - auto data_new = reinterpret_cast(data); - std::size_t n = frame->width() * frame->height(); - for (std::size_t i = 0; i < n; i++) { - frame->data()[i] = *(data_new + (i * 2 + 1)); +#pragma pack(push, 1) +struct ImagePacket { + std::uint8_t header; + std::uint8_t size; + std::uint16_t frame_id; + std::uint32_t timestamp; + std::uint16_t exposure_time; + std::uint8_t checksum; + + ImagePacket() = default; + explicit ImagePacket(std::uint8_t *data) { + from_data(data); } - return true; -} + + void from_data(std::uint8_t *data) { + header = *data; + size = *(data + 1); + frame_id = (*(data + 2) << 8) | *(data + 3); + timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) | + *(data + 7); + exposure_time = (*(data + 8) << 8) | *(data + 9); + checksum = *(data + 10); + } +}; +#pragma pack(pop) bool unpack_stereo_img_data( const void *data, const StreamRequest &request, ImgData *img) { @@ -57,11 +59,11 @@ bool unpack_stereo_img_data( request.width * request.height * bytes_per_pixel(request.format); auto data_end = data_new + data_n; - std::size_t packet_n = sizeof(ImagePacketS1); + std::size_t packet_n = sizeof(ImagePacket); std::vector packet(packet_n); std::reverse_copy(data_end - packet_n, data_end, packet.begin()); - ImagePacketS1 img_packet(packet.data()); + ImagePacket img_packet(packet.data()); // LOG(INFO) << "ImagePacket: header=0x" << std::hex << // static_cast(img_packet.header) // << ", size=0x" << std::hex << static_cast(img_packet.size) @@ -97,6 +99,34 @@ bool unpack_stereo_img_data( return true; } +// 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::YUYV); + CHECK_EQ(frame->format(), Format::GREY); + auto data_new = reinterpret_cast(data); + std::size_t n = frame->width() * frame->height(); + for (std::size_t i = 0; i < n; i++) { + frame->data()[i] = *(data_new + (i * 2)); + } + 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::YUYV); + CHECK_EQ(frame->format(), Format::GREY); + auto data_new = reinterpret_cast(data); + std::size_t n = frame->width() * frame->height(); + for (std::size_t i = 0; i < n; i++) { + frame->data()[i] = *(data_new + (i * 2 + 1)); + } + return true; +} + } // namespace StandardStreamsAdapter::StandardStreamsAdapter() { diff --git a/src/mynteye/device/standard2/streams_adapter_s2.cc b/src/mynteye/device/standard2/streams_adapter_s2.cc index a612160..1319965 100644 --- a/src/mynteye/device/standard2/streams_adapter_s2.cc +++ b/src/mynteye/device/standard2/streams_adapter_s2.cc @@ -22,6 +22,42 @@ 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(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); @@ -75,11 +111,11 @@ bool unpack_stereo_img_data( request.width * request.height * bytes_per_pixel(request.format); auto data_end = data_new + data_n; - std::size_t packet_n = sizeof(ImagePacketS2); + std::size_t packet_n = sizeof(ImagePacket); std::vector packet(packet_n); std::reverse_copy(data_end - packet_n, data_end, packet.begin()); - ImagePacketS2 img_packet(packet.data()); + ImagePacket img_packet(packet.data()); // LOG(INFO) << "ImagePacket: header=0x" << std::hex << // static_cast(img_packet.header) // << ", size=0x" << std::hex << static_cast(img_packet.size) diff --git a/src/mynteye/device/types.h b/src/mynteye/device/types.h index cd216b0..77ec08d 100644 --- a/src/mynteye/device/types.h +++ b/src/mynteye/device/types.h @@ -138,72 +138,6 @@ struct MYNTEYE_API DeviceInfo { std::uint16_t nominal_baseline; }; -/** - * @ingroup datatypes - * Image packet for standard 2. - */ -#pragma pack(push, 1) -struct ImagePacketS2 { - 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; - - ImagePacketS2() = default; - explicit ImagePacketS2(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(timestamp_h) << 32) | timestamp_l; - exposure_time = (*(data + 12) << 8) | *(data + 13); - checksum = *(data + 14); - } -}; -#pragma pack(pop) - -/** - * @ingroup datatypes - * Image packet for standard 1. - */ -#pragma pack(push, 1) -struct ImagePacketS1 { - std::uint8_t header; - std::uint8_t size; - std::uint16_t frame_id; - std::uint32_t timestamp; - std::uint16_t exposure_time; - std::uint8_t checksum; - - ImagePacketS1() = default; - explicit ImagePacketS1(std::uint8_t *data) { - from_data(data); - } - - void from_data(std::uint8_t *data) { - header = *data; - size = *(data + 1); - frame_id = (*(data + 2) << 8) | *(data + 3); - timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) | - *(data + 7); - exposure_time = (*(data + 8) << 8) | *(data + 9); - checksum = *(data + 10); - } -}; -#pragma pack(pop) - /** * @ingroup datatypes * Imu request packet.