From 490185a5778b52cc810fafbb0e13e18b7d7dbb5b Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Tue, 10 Apr 2018 00:26:22 +0800
Subject: [PATCH] Add options get set

---
 include/mynteye/types.h  |  10 +++
 samples/device/camera.cc |   2 +
 src/device/device.cc     |  17 ++++
 src/device/device.h      |   5 ++
 src/internal/channels.cc | 188 ++++++++++++++++++++++++++++++++++++++-
 src/internal/channels.h  |  23 ++++-
 6 files changed, 240 insertions(+), 5 deletions(-)

diff --git a/include/mynteye/types.h b/include/mynteye/types.h
index b81a07f..f34cf11 100644
--- a/include/mynteye/types.h
+++ b/include/mynteye/types.h
@@ -391,6 +391,16 @@ struct MYNTEYE_API ImuData {
   }
 };
 
+/**
+ * @ingroup datatypes
+ * Option info.
+ */
+struct MYNTEYE_API OptionInfo {
+  std::int32_t min;
+  std::int32_t max;
+  std::int32_t def;
+};
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_TYPES_H_ NOLINT
diff --git a/samples/device/camera.cc b/samples/device/camera.cc
index 94c009e..0544960 100644
--- a/samples/device/camera.cc
+++ b/samples/device/camera.cc
@@ -42,6 +42,8 @@ int main(int argc, char *argv[]) {
     }
   }
 
+  device->LogOptionInfos();
+
   std::size_t left_count = 0;
   device->SetStreamCallback(
       Stream::LEFT, [&left_count](const device::StreamData &data) {
diff --git a/src/device/device.cc b/src/device/device.cc
index 28afe38..268361d 100644
--- a/src/device/device.cc
+++ b/src/device/device.cc
@@ -131,6 +131,23 @@ ImuExtrinsics Device::GetImuExtrinsics() const {
   return imu_extrinsics_;
 }
 
+OptionInfo Device::GetOptionInfo(const Option &option) const {
+  auto &&info = channels_->GetControlInfo(option);
+  return {info.min, info.max, info.def};
+}
+
+std::int32_t Device::GetOptionValue(const Option &option) const {
+  return channels_->GetControlValue(option);
+}
+
+void Device::SetOptionValue(const Option &option, std::int32_t value) {
+  channels_->SetControlValue(option, value);
+}
+
+void Device::LogOptionInfos() const {
+  channels_->LogControlInfos();
+}
+
 void Device::SetStreamCallback(
     const Stream &stream, stream_callback_t callback) {
   if (!Supports(stream)) {
diff --git a/src/device/device.h b/src/device/device.h
index f2e3a03..d9061c6 100644
--- a/src/device/device.h
+++ b/src/device/device.h
@@ -61,6 +61,11 @@ class Device {
   ImuIntrinsics GetImuIntrinsics() const;
   ImuExtrinsics GetImuExtrinsics() const;
 
+  OptionInfo GetOptionInfo(const Option &option) const;
+  std::int32_t GetOptionValue(const Option &option) const;
+  void SetOptionValue(const Option &option, std::int32_t value);
+  void LogOptionInfos() const;
+
   void SetStreamCallback(const Stream &stream, stream_callback_t callback);
   void SetMotionCallback(motion_callback_t callback);
 
diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index 5b99b43..43ece39 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -2,26 +2,210 @@
 
 #include <glog/logging.h>
 
+#include <stdexcept>
+
 MYNTEYE_BEGIN_NAMESPACE
 
+namespace {
+
+int XuCamCtrlId(Option option) {
+  switch (option) {
+    case Option::EXPOSURE_MODE:
+      return 0;
+      break;
+    case Option::MAX_GAIN:
+      return 1;
+      break;
+    case Option::MAX_EXPOSURE_TIME:
+      return 2;
+      break;
+    case Option::DESIRED_BRIGHTNESS:
+      return 3;
+      break;
+    case Option::IMU_FREQUENCY:
+      return 4;
+      break;
+    case Option::IR_CONTROL:
+      return 5;
+      break;
+    case Option::HDR_MODE:
+      return 6;
+      break;
+    case Option::FRAME_RATE:
+      return 7;
+      break;
+    default:
+      LOG(FATAL) << "No cam ctrl id for " << option;
+  }
+}
+
+}  // namespace
+
 Channels::Channels(std::shared_ptr<uvc::device> device) : device_(device) {
   VLOG(2) << __func__;
+
+  for (auto &&option : std::vector<Option>{Option::GAIN, Option::BRIGHTNESS,
+                                           Option::CONTRAST}) {
+    control_infos_[option] = PuControlInfo(option);
+  }
+
+  for (auto &&option : std::vector<Option>{
+           Option::FRAME_RATE, Option::IMU_FREQUENCY, Option::EXPOSURE_MODE,
+           Option::MAX_GAIN, Option::MAX_EXPOSURE_TIME,
+           Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE}) {
+    control_infos_[option] = XuControlInfo(option);
+  }
+
+  if (VLOG_IS_ON(2)) {
+    for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++) {
+      VLOG(2) << it->first << ": min=" << it->second.min
+              << ", max=" << it->second.max << ", def=" << it->second.def
+              << ", cur=" << GetControlValue(it->first);
+    }
+  }
 }
 
 Channels::~Channels() {
   VLOG(2) << __func__;
 }
 
+void Channels::LogControlInfos() const {
+  for (auto &&it = control_infos_.begin(); it != control_infos_.end(); it++) {
+    LOG(INFO) << it->first << ": min=" << it->second.min
+              << ", max=" << it->second.max << ", def=" << it->second.def
+              << ", cur=" << GetControlValue(it->first);
+  }
+}
+
+Channels::control_info_t Channels::GetControlInfo(const Option &option) const {
+  try {
+    return control_infos_.at(option);
+  } catch (const std::out_of_range &e) {
+    return {0, 0, 0};
+  }
+}
+
+std::int32_t Channels::GetControlValue(const Option &option) const {
+  switch (option) {
+    case Option::GAIN:
+    case Option::BRIGHTNESS:
+    case Option::CONTRAST:
+      std::int32_t value;
+      if (!PuControlQuery(option, uvc::PU_QUERY_GET, &value)) {
+        LOG(WARNING) << option << " get value failed";
+        return -1;
+      } else {
+        return value;
+      }
+    case Option::FRAME_RATE:
+    case Option::IMU_FREQUENCY:
+    case Option::EXPOSURE_MODE:
+    case Option::MAX_GAIN:
+    case Option::MAX_EXPOSURE_TIME:
+    case Option::DESIRED_BRIGHTNESS:
+    case Option::IR_CONTROL:
+    case Option::HDR_MODE:
+      return XuCamCtrlGet(option);
+    case Option::ZERO_DRIFT_CALIBRATION:
+    case Option::ERASE_CHIP:
+      LOG(WARNING) << option << " get value useless";
+      return -1;
+    default:
+      LOG(FATAL) << "Unsupported option " << option;
+  }
+  return -1;
+}
+
+void Channels::SetControlValue(const Option &option, std::int32_t value) {
+  // TODO(JohnZhao)
+  UNUSED(option)
+  UNUSED(value)
+}
+
+bool Channels::PuControlRange(
+    Option option, int32_t *min, int32_t *max, int32_t *def) const {
+  CHECK_NOTNULL(device_);
+  return uvc::pu_control_range(*device_, option, min, max, def);
+}
+
+bool Channels::PuControlQuery(
+    Option option, uvc::pu_query query, int32_t *value) const {
+  CHECK_NOTNULL(device_);
+  return uvc::pu_control_query(*device_, option, query, value);
+}
+
 bool Channels::XuControlQuery(
-    uint8_t selector, uvc::xu_query query, uint16_t size, uint8_t *data) {
+    uint8_t selector, uvc::xu_query query, uint16_t size, uint8_t *data) const {
   return XuControlQuery({3}, selector, query, size, data);
 }
 
 bool Channels::XuControlQuery(
     const uvc::xu &xu, uint8_t selector, uvc::xu_query query, uint16_t size,
-    uint8_t *data) {
+    uint8_t *data) const {
   CHECK_NOTNULL(device_);
   return uvc::xu_control_query(*device_, xu, selector, query, size, data);
 }
 
+bool Channels::XuCamCtrlQuery(
+    uvc::xu_query query, uint16_t size, uint8_t *data) const {
+  return XuControlQuery(CHANNEL_CAM_CTRL >> 8, query, size, data);
+}
+
+std::int32_t Channels::XuCamCtrlGet(Option option) const {
+  int id = XuCamCtrlId(option);
+
+  std::uint8_t data[3] = {static_cast<std::uint8_t>((id | 0x80) & 0xFF), 0, 0};
+  if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
+    LOG(WARNING) << "Get control value of " << option << " failed";
+    return -1;
+  }
+
+  data[0] = id & 0xFF;
+  if (XuCamCtrlQuery(uvc::XU_QUERY_GET, 3, data)) {
+    return (data[1] << 8) + (data[2]);
+  } else {
+    LOG(WARNING) << "Get control value of " << option << " failed";
+    return -1;
+  }
+}
+
+Channels::control_info_t Channels::PuControlInfo(Option option) const {
+  int32_t min = 0, max = 0, def = 0;
+  if (!PuControlRange(option, &min, &max, &def)) {
+    LOG(WARNING) << "Get control range of " << option << " failed";
+  }
+  return {min, max, def};
+}
+
+Channels::control_info_t Channels::XuControlInfo(Option option) const {
+  int id = XuCamCtrlId(option);
+
+  std::uint8_t data[3] = {static_cast<std::uint8_t>((id | 0x80) & 0xFF), 0, 0};
+  if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
+    LOG(WARNING) << "Get control range of " << option << " failed";
+    return {0, 0, 0};
+  }
+
+  control_info_t info{0, 0, 0};
+
+  data[0] = id & 0xFF;
+  if (XuCamCtrlQuery(uvc::XU_QUERY_MIN, 3, data)) {
+    info.min = (data[1] << 8) + (data[2]);
+  } else {
+    LOG(WARNING) << "Get control range min of " << option << " failed";
+  }
+  if (XuCamCtrlQuery(uvc::XU_QUERY_MAX, 3, data)) {
+    info.max = (data[1] << 8) + (data[2]);
+  } else {
+    LOG(WARNING) << "Get control range max of " << option << " failed";
+  }
+  if (XuCamCtrlQuery(uvc::XU_QUERY_DEF, 3, data)) {
+    info.def = (data[1] << 8) + (data[2]);
+  } else {
+    LOG(WARNING) << "Get control range def of " << option << " failed";
+  }
+
+  return info;
+}
+
 MYNTEYE_END_NAMESPACE
diff --git a/src/internal/channels.h b/src/internal/channels.h
index 467029d..0f975e5 100644
--- a/src/internal/channels.h
+++ b/src/internal/channels.h
@@ -37,16 +37,33 @@ class Channels {
   explicit Channels(std::shared_ptr<uvc::device> device);
   ~Channels();
 
+  void LogControlInfos() const;
+
+  control_info_t GetControlInfo(const Option &option) const;
+  std::int32_t GetControlValue(const Option &option) const;
+  void SetControlValue(const Option &option, std::int32_t value);
+
  private:
+  bool PuControlRange(
+      Option option, int32_t *min, int32_t *max, int32_t *def) const;
+  bool PuControlQuery(Option option, uvc::pu_query query, int32_t *value) const;
+
   bool XuControlQuery(
-      uint8_t selector, uvc::xu_query query, uint16_t size, uint8_t *data);
+      uint8_t selector, uvc::xu_query query, uint16_t size,
+      uint8_t *data) const;
   bool XuControlQuery(
       const uvc::xu &xu, uint8_t selector, uvc::xu_query query, uint16_t size,
-      uint8_t *data);
+      uint8_t *data) const;
+
+  bool XuCamCtrlQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
+  std::int32_t XuCamCtrlGet(Option option) const;
+
+  control_info_t PuControlInfo(Option option) const;
+  control_info_t XuControlInfo(Option option) const;
 
   std::shared_ptr<uvc::device> device_;
 
-  std::map<Option, ControlInfo> control_infos_;
+  std::map<Option, control_info_t> control_infos_;
 };
 
 MYNTEYE_END_NAMESPACE