feat(root): add s210a

CMakeLists.txt

@@ -176,6 +176,9 @@ 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

include/mynteye/types.h

@@ -37,8 +37,10 @@ MYNTEYE_BEGIN_NAMESPACE
 enum class Model : std::uint8_t {
   /** Standard */
   STANDARD,
-  /** Standard 2 generation */
+  /** Standard 2 */
   STANDARD2,
+  /** Standard 210a */
+  STANDARD210A,
   /** Last guard */
   LAST
 };
@@ -313,6 +315,8 @@ enum class Format : std::uint32_t {
   YUYV = MYNTEYE_FOURCC('Y', 'U', 'Y', 'V'),
   /** BGR 8:8:8, 24 bits per pixel */
   BGR888 = MYNTEYE_FOURCC('B', 'G', 'R', '3'),
+  /** RGB 8:8:8, 24 bits per pixel */
+  RGB888 = MYNTEYE_FOURCC('R', 'G', 'B', '3'),
   /** Last guard */
   LAST
 };

samples/tutorials/control/auto_exposure.cc

@@ -58,8 +58,8 @@ int main(int argc, char *argv[]) {
               << api->GetOptionValue(Option::DESIRED_BRIGHTNESS);
   }
 
-  // Set auto exposure options fo s210a or s2000
+  // Set auto exposure options fo S2000/S2100/S210A
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
     // auto-exposure: 0
     api->SetOptionValue(Option::EXPOSURE_MODE, 0);
 

samples/tutorials/control/framerate.cc

@@ -49,8 +49,8 @@ int main(int argc, char *argv[]) {
               << api->GetOptionValue(Option::IMU_FREQUENCY);
   }
 
-  // You should set frame rate for S210A by 'SelectStreamRequest()'
+  // You should set frame rate for S2000/S2100/S210A by 'SelectStreamRequest()'
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
     LOG(INFO) << "Please set frame rate by 'SelectStreamRequest()'";
   }
 

samples/tutorials/control/imu_low_pass_filter.cc

@@ -38,8 +38,8 @@ int main(int argc, char *argv[]) {
     return 0;
   }
 
-  // Set imu low pass filter for s210a
+  // Set imu low pass filter for S2000/S2100/S210A
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
     // ACCELEROMETER_RANGE values: 0, 1, 2
     api->SetOptionValue(Option::ACCELEROMETER_LOW_PASS_FILTER, 2);
     // GYROSCOPE_RANGE values: 23, 64

samples/tutorials/control/imu_range.cc

@@ -32,7 +32,7 @@ int main(int argc, char *argv[]) {
 
   Model model = api->GetModel();
 
-  // Set imu range for s1030
+  // Set imu range for S1030
   if (model == Model::STANDARD) {
     // ACCELEROMETER_RANGE values: 4, 8, 16, 32
     api->SetOptionValue(Option::ACCELEROMETER_RANGE, 8);
@@ -40,8 +40,8 @@ int main(int argc, char *argv[]) {
     api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000);
   }
 
-  // Set imu range for s210a
+  // Set imu range for S2000/S2100/S210A
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
     // ACCELEROMETER_RANGE values: 6, 12, 24, 32
     api->SetOptionValue(Option::ACCELEROMETER_RANGE, 6);
     // GYROSCOPE_RANGE values: 250, 500, 1000, 2000, 4000

samples/tutorials/control/infrared.cc

@@ -29,8 +29,7 @@ int main(int argc, char *argv[]) {
 
   Model model = api->GetModel();
 
-  // Set ir low pass filter for s210a
+  if (model == Model::STANDARD || model == Model::STANDARD2) {
-  if (model == Model::STANDARD) {
     // ir control: range [0,160], default 0
     api->SetOptionValue(Option::IR_CONTROL, 80);
 
@@ -39,7 +38,7 @@ int main(int argc, char *argv[]) {
   }
 
   // MYNTEYE-S210A don't support this option
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD210A) {
     LOG(INFO) << "Sorry,MYNTEYE-S210A don't support ir control";
     return 0;
   }

samples/tutorials/control/manual_exposure.cc

@@ -54,8 +54,8 @@ int main(int argc, char *argv[]) {
     LOG(INFO) << "Set CONTRAST to " << api->GetOptionValue(Option::CONTRAST);
   }
 
-  // Set manual exposure options fo s210a or s2000
+  // Set manual exposure options fo S2000/S2100/S210A
-  if (model == Model::STANDARD2) {
+  if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
     // manual-exposure: 1
     api->SetOptionValue(Option::EXPOSURE_MODE, 1);
 

src/mynteye/device/config.cc

@@ -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},

src/mynteye/device/device.cc

@@ -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) {
+      if (model.generation == '1') {
-        case '1':
+        return std::make_shared<StandardDevice>(device);
-          return std::make_shared<StandardDevice>(device);
+      } else if (model.generation == '2') {
-        case '2':
+        if (model.custom_code == '0') {
           return std::make_shared<Standard2Device>(device);
-        default:
+        } else if (model.custom_code == 'A') {
-          LOG(FATAL) << "No such generation now";
+          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";

src/mynteye/device/standard2/channels_adapter_s210a.cc

@@ -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 &&params = (*img_params).at({1280, 400});
+    i += bytes::to_data(&params.in_left, data + i, version);
+    i += bytes::to_data(&params.in_right, data + i, version);
+  }
+  {
+    auto &&params = (*img_params).at({2560, 800});
+    i += bytes::to_data(&params.in_left, data + i, version);
+    i += bytes::to_data(&params.in_right, data + i, version);
+    i += bytes::to_data(&params.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

src/mynteye/device/standard2/channels_adapter_s210a.h

@@ -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_

src/mynteye/device/standard2/device_s210a.cc

@@ -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

src/mynteye/device/standard2/device_s210a.h

@@ -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_

src/mynteye/device/standard2/streams_adapter_s2.cc

@@ -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(request.format, Format::YUYV);
-  CHECK_EQ(frame->format(), Format::BGR888);
+  CHECK_EQ(frame->format(), Format::YUYV);
   auto data_new = reinterpret_cast<const std::uint8_t *>(data);
-  std::size_t n = 3;
+  std::size_t n = 2;
-  std::size_t w = frame->width();
+  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] =
+      frame->data()[i * w + j] = *(data_new + 2 * i * w + j);
-        *(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;
@@ -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(request.format, Format::YUYV);
-  CHECK_EQ(frame->format(), Format::BGR888);
+  CHECK_EQ(frame->format(), Format::YUYV);
   auto data_new = reinterpret_cast<const std::uint8_t *>(data);
-  std::size_t n = 3;
+  std::size_t n = 2;
-  std::size_t w = frame->width();
+  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] =
+      frame->data()[i * w + j] = *(data_new + (2 * i + 1) * w + j);
-        *(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;

src/mynteye/device/standard2/streams_adapter_s210a.cc

@@ -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

src/mynteye/device/standard2/streams_adapter_s210a.h

@@ -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_

src/mynteye/types.cc

@@ -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";

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -37,22 +37,24 @@
 
   <!-- Request index -->
 
-  <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 10 -->
+  <!-- Attention:The format of MYNTEYE-S2100/S2000 is YUYV.S210A's format is BGR888 -->
-  <arg name="index_s210a_0"   default="0" />
-  <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 20 -->
-  <arg name="index_s210a_1"   default="1" />
-  <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 30 -->
-  <arg name="index_s210a_2"   default="2" />
-  <!-- MYNTEYE-S210A, Reslution: 1280x400, Format: BGR888, Fps: 60 -->
-  <arg name="index_s210a_3"   default="3" />
-  <!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 10 -->
-  <arg name="index_s210a_4"   default="4" />
-  <!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 20 -->
-  <arg name="index_s210a_5"   default="5" />
-  <!-- MYNTEYE-S210A, Reslution: 2560x800, Format: BGR888, Fps: 30 -->
-  <arg name="index_s210a_6"   default="6" />
 
-  <arg name="request_index" default="$(arg index_s210a_2)" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 10 -->
+  <arg name="index_s2_0"   default="0" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 20 -->
+  <arg name="index_s2_1"   default="1" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 30 -->
+  <arg name="index_s2_2"   default="2" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 1280x400, Format: YUYV/BGR888, Fps: 60 -->
+  <arg name="index_s2_3"   default="3" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 10 -->
+  <arg name="index_s2_4"   default="4" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 20 -->
+  <arg name="index_s2_5"   default="5" />
+  <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30 -->
+  <arg name="index_s2_6"   default="6" />
+
+  <arg name="request_index" default="$(arg index_s2_2)" />
 
   <arg name="enable_left_rect" default="false" />
   <arg name="enable_right_rect" default="false" />
@@ -141,6 +143,10 @@
   <arg name="standard2/min_exposure_time" default="-1" />
   <!-- <arg name="standard2/min_exposure_time" default="0" /> -->
 
+  <!-- standard2/ir_control range: [0,160] -->
+  <arg name="standard2/ir_control" default="80" />
+  <!-- <arg name="standard2/ir_control" default="0" /> -->
+
   <!-- standard2/accel_range range: [6,48] -->
   <arg name="standard2/accel_range" default="-1" />
   <!-- <arg name="standard2/accel_range" default="6" /> -->
@@ -157,6 +163,48 @@
   <arg name="standard2/gyro_low_filter" default="-1" />
   <!-- <arg name="standard2/gyro_low_filter" default="64" /> -->
 
+  <!-- device options of standard210a, -1 will not set the value -->
+
+  <!-- standard210a/brightness range: [0,240] -->
+  <arg name="standard210a/brightness" default="-1" />
+  <!-- <arg name="standard210a/brightness" default="120" /> -->
+
+  <!-- standard210a/exposure_mode, 0: auto-exposure, 1: manual-exposure -->
+  <arg name="standard210a/exposure_mode" default="-1" />
+  <!-- <arg name="standard210a/exposure_mode" default="0" /> -->
+
+  <!-- standard210a/max_gain range: [0,255] -->
+  <arg name="standard210a/max_gain" default="-1" />
+  <!-- <arg name="standard210a/max_gain" default="8" /> -->
+
+  <!-- standard210a/max_exposure_time range: [0,1000] -->
+  <arg name="standard210a/max_exposure_time" default="-1" />
+  <!-- <arg name="standard210a/max_exposure_time" default="333" /> -->
+
+  <!-- standard210a/desired_brightness range: [1,255] -->
+  <arg name="standard210a/desired_brightness" default="-1" />
+  <!-- <arg name="standard210a/desired_brightness" default="122" /> -->
+
+  <!-- standard210a/min_exposure_time range: [0,1000] -->
+  <arg name="standard210a/min_exposure_time" default="-1" />
+  <!-- <arg name="standard210a/min_exposure_time" default="0" /> -->
+
+  <!-- standard210a/accel_range range: [6,48] -->
+  <arg name="standard210a/accel_range" default="-1" />
+  <!-- <arg name="standard210a/accel_range" default="6" /> -->
+
+  <!-- standard210a/gyro_range range: [250,4000] -->
+  <arg name="standard210a/gyro_range" default="-1" />
+  <!-- <arg name="standard210a/gyro_range" default="1000" /> -->
+
+  <!-- standard210a/accel_low_filter range: [0,2] -->
+  <arg name="standard210a/accel_low_filter" default="-1" />
+  <!-- <arg name="standard210a/accel_low_filter" default="2" /> -->
+
+  <!-- standard210a/gyro_low_filter range: [23,64] -->
+  <arg name="standard210a/gyro_low_filter" default="-1" />
+  <!-- <arg name="standard210a/gyro_low_filter" default="64" /> -->
+
   <!-- Push down all topics/nodelets into "mynteye" namespace -->
   <group ns="$(arg mynteye)">
 

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -220,6 +220,22 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     gravity_ = 9.8;
     private_nh_.getParam("gravity", gravity_);
 
+    // device options of standard210a
+    if (model_ == Model::STANDARD210A) {
+      option_names_ = {
+          {Option::BRIGHTNESS, "standard210a/brightness"},
+          {Option::EXPOSURE_MODE, "standard210a/exposure_mode"},
+          {Option::MAX_GAIN, "standard210a/max_gain"},
+          {Option::MAX_EXPOSURE_TIME, "standard210a/max_exposure_time"},
+          {Option::DESIRED_BRIGHTNESS, "standard210a/desired_brightness"},
+          {Option::MIN_EXPOSURE_TIME, "standard210a/min_exposure_time"},
+          {Option::ACCELEROMETER_RANGE, "standard210a/accel_range"},
+          {Option::GYROSCOPE_RANGE, "standard210a/gyro_range"},
+          {Option::ACCELEROMETER_LOW_PASS_FILTER,
+                  "standard210a/accel_low_filter"},
+          {Option::GYROSCOPE_LOW_PASS_FILTER, "standard210a/gyro_low_filter"}};
+    }
+
     // device options of standard2
     if (model_ == Model::STANDARD2) {
       option_names_ = {
@@ -229,6 +245,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
           {Option::MAX_EXPOSURE_TIME, "standard2/max_exposure_time"},
           {Option::DESIRED_BRIGHTNESS, "standard2/desired_brightness"},
           {Option::MIN_EXPOSURE_TIME, "standard2/min_exposure_time"},
+          {Option::IR_CONTROL, "STANDARD/ir_control"},
           {Option::ACCELEROMETER_RANGE, "standard2/accel_range"},
           {Option::GYROSCOPE_RANGE, "standard2/gyro_range"},
           {Option::ACCELEROMETER_LOW_PASS_FILTER, "standard2/accel_low_filter"},
@@ -237,19 +254,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     // device options of standard
     if (model_ == Model::STANDARD) {
       option_names_ = {
-          {Option::GAIN, "STANDARD/gain"},
+          {Option::GAIN, "standard/gain"},
-          {Option::BRIGHTNESS, "STANDARD/brightness"},
+          {Option::BRIGHTNESS, "standard/brightness"},
-          {Option::CONTRAST, "STANDARD/contrast"},
+          {Option::CONTRAST, "standard/contrast"},
-          {Option::FRAME_RATE, "STANDARD/frame_rate"},
+          {Option::FRAME_RATE, "standard/frame_rate"},
-          {Option::IMU_FREQUENCY, "STANDARD/imu_frequency"},
+          {Option::IMU_FREQUENCY, "standard/imu_frequency"},
-          {Option::EXPOSURE_MODE, "STANDARD/exposure_mode"},
+          {Option::EXPOSURE_MODE, "standard/exposure_mode"},
-          {Option::MAX_GAIN, "STANDARD/max_gain"},
+          {Option::MAX_GAIN, "standard/max_gain"},
-          {Option::MAX_EXPOSURE_TIME, "STANDARD/max_exposure_time"},
+          {Option::MAX_EXPOSURE_TIME, "standard/max_exposure_time"},
-          {Option::DESIRED_BRIGHTNESS, "STANDARD/desired_brightness"},
+          {Option::DESIRED_BRIGHTNESS, "standard/desired_brightness"},
-          {Option::IR_CONTROL, "STANDARD/ir_control"},
+          {Option::IR_CONTROL, "standard/ir_control"},
-          {Option::HDR_MODE, "STANDARD/hdr_mode"},
+          {Option::HDR_MODE, "standard/hdr_mode"},
-          {Option::ACCELEROMETER_RANGE, "STANDARD/accel_range"},
+          {Option::ACCELEROMETER_RANGE, "standard/accel_range"},
-          {Option::GYROSCOPE_RANGE, "STANDARD/gyro_range"}};
+          {Option::GYROSCOPE_RANGE, "standard/gyro_range"}};
     }
 
     for (auto &&it = option_names_.begin(); it != option_names_.end(); ++it) {
@@ -278,8 +295,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       NODELET_INFO_STREAM("Advertized on topic " << topic);
     }
 
-    // Only STANDARD2 need publish mono_topics
+    // Only STANDARD2/STANDARD210A need publish mono_topics
-    if (model_ == Model::STANDARD2) {
+    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
       for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
         auto &&topic = mono_topics[it->first];
         if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
@@ -288,7 +305,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
         NODELET_INFO_STREAM("Advertized on topic " << topic);
       }
     }
-    if (model_ == Model::STANDARD2) {
+
+    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
       camera_encodings_ = {{Stream::LEFT, enc::BGR8},
                           {Stream::RIGHT, enc::BGR8},
                           {Stream::LEFT_RECTIFIED, enc::BGR8},
@@ -862,7 +880,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     int request_index = 0;
 
     model_ = api_->GetModel();
-    if (model_ == Model::STANDARD2) {
+    if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
       private_nh_.getParam("request_index", request_index);
       switch (request_index) {
         case 0: