From 8c0605255bb1de8ca56a1f92a81f2a86810e8032 Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Tue, 10 Apr 2018 13:58:37 +0800
Subject: [PATCH] Complete options get set

---
 samples/device/camera.cc | 17 +++++++++
 src/device/device.cc     | 16 +++++++-
 src/internal/channels.cc | 79 ++++++++++++++++++++++++++++++++--------
 src/internal/channels.h  |  3 ++
 4 files changed, 98 insertions(+), 17 deletions(-)

diff --git a/samples/device/camera.cc b/samples/device/camera.cc
index 0544960..f415a41 100644
--- a/samples/device/camera.cc
+++ b/samples/device/camera.cc
@@ -42,6 +42,23 @@ int main(int argc, char *argv[]) {
     }
   }
 
+  /*
+  {  // auto-exposure
+    device->SetOptionValue(Option::EXPOSURE_MODE, 0);
+    device->SetOptionValue(Option::MAX_GAIN, 40);  // [0.48]
+    device->SetOptionValue(Option::MAX_EXPOSURE_TIME, 120);  // [0,240]
+    device->SetOptionValue(Option::DESIRED_BRIGHTNESS, 200);  // [0,255]
+  }
+  */
+  /*
+  {  // manual-exposure
+    device->SetOptionValue(Option::EXPOSURE_MODE, 1);
+    device->SetOptionValue(Option::GAIN, 20);  // [0.48]
+    device->SetOptionValue(Option::BRIGHTNESS, 20);  // [0,240]
+    device->SetOptionValue(Option::CONTRAST, 20);  // [0,255]
+  }
+  device->SetOptionValue(Option::IR_CONTROL, 80);
+  */
   device->LogOptionInfos();
 
   std::size_t left_count = 0;
diff --git a/src/device/device.cc b/src/device/device.cc
index 268361d..3dbd9bd 100644
--- a/src/device/device.cc
+++ b/src/device/device.cc
@@ -58,7 +58,7 @@ bool Device::Supports(const Option &option) const {
 const std::vector<StreamRequest> &Device::GetStreamRequests(
     const Capabilities &capability) const {
   if (!Supports(capability)) {
-    LOG(FATAL) << "Unsupported capability: " << to_string(capability);
+    LOG(FATAL) << "Unsupported capability: " << capability;
   }
   try {
     auto &&cap_requests = stream_requests_map.at(model_);
@@ -132,15 +132,27 @@ ImuExtrinsics Device::GetImuExtrinsics() const {
 }
 
 OptionInfo Device::GetOptionInfo(const Option &option) const {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return {0, 0, 0};
+  }
   auto &&info = channels_->GetControlInfo(option);
   return {info.min, info.max, info.def};
 }
 
 std::int32_t Device::GetOptionValue(const Option &option) const {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return -1;
+  }
   return channels_->GetControlValue(option);
 }
 
 void Device::SetOptionValue(const Option &option, std::int32_t value) {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return;
+  }
   channels_->SetControlValue(option, value);
 }
 
@@ -151,7 +163,7 @@ void Device::LogOptionInfos() const {
 void Device::SetStreamCallback(
     const Stream &stream, stream_callback_t callback) {
   if (!Supports(stream)) {
-    LOG(WARNING) << "Unsupported stream: " << to_string(stream);
+    LOG(WARNING) << "Unsupported stream: " << stream;
     return;
   }
   if (callback) {
diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index 43ece39..5b18002 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -43,7 +43,22 @@ int XuCamCtrlId(Option option) {
 
 Channels::Channels(std::shared_ptr<uvc::device> device) : device_(device) {
   VLOG(2) << __func__;
+  UpdateControlInfos();
+}
 
+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);
+  }
+}
+
+void Channels::UpdateControlInfos() {
   for (auto &&option : std::vector<Option>{Option::GAIN, Option::BRIGHTNESS,
                                            Option::CONTRAST}) {
     control_infos_[option] = PuControlInfo(option);
@@ -65,22 +80,11 @@ Channels::Channels(std::shared_ptr<uvc::device> device) : device_(device) {
   }
 }
 
-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) {
+    LOG(WARNING) << "Get control info of " << option << " failed";
     return {0, 0, 0};
   }
 }
@@ -117,9 +121,44 @@ std::int32_t Channels::GetControlValue(const Option &option) const {
 }
 
 void Channels::SetControlValue(const Option &option, std::int32_t value) {
-  // TODO(JohnZhao)
-  UNUSED(option)
-  UNUSED(value)
+  auto in_range = [this, &option, &value]() {
+    auto &&info = GetControlInfo(option);
+    if (value < info.min || value > info.max) {
+      LOG(WARNING) << option << " set value out of range, " << value
+                   << " not in [" << info.min << "," << info.max << "]";
+      return false;
+    }
+    return true;
+  };
+  switch (option) {
+    case Option::GAIN:
+    case Option::BRIGHTNESS:
+    case Option::CONTRAST: {
+      if (!in_range())
+        break;
+      if (!PuControlQuery(option, uvc::PU_QUERY_SET, &value)) {
+        LOG(WARNING) << option << " set value failed";
+      }
+    } break;
+    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: {
+      if (!in_range())
+        break;
+      XuCamCtrlSet(option, value);
+    } break;
+    case Option::ZERO_DRIFT_CALIBRATION:
+    case Option::ERASE_CHIP:
+      LOG(WARNING) << option << " set value useless";
+      break;
+    default:
+      LOG(FATAL) << "Unsupported option " << option;
+  }
 }
 
 bool Channels::PuControlRange(
@@ -169,6 +208,16 @@ std::int32_t Channels::XuCamCtrlGet(Option option) const {
   }
 }
 
+void Channels::XuCamCtrlSet(Option option, std::int32_t value) const {
+  int id = XuCamCtrlId(option);
+  std::uint8_t data[3] = {static_cast<std::uint8_t>(id & 0xFF),
+                          static_cast<std::uint8_t>((value >> 8) & 0xFF),
+                          static_cast<std::uint8_t>(value & 0xFF)};
+  if (!XuCamCtrlQuery(uvc::XU_QUERY_SET, 3, data)) {
+    LOG(WARNING) << "Set control value of " << option << " failed";
+  }
+}
+
 Channels::control_info_t Channels::PuControlInfo(Option option) const {
   int32_t min = 0, max = 0, def = 0;
   if (!PuControlRange(option, &min, &max, &def)) {
diff --git a/src/internal/channels.h b/src/internal/channels.h
index 0f975e5..6c43619 100644
--- a/src/internal/channels.h
+++ b/src/internal/channels.h
@@ -39,6 +39,8 @@ class Channels {
 
   void LogControlInfos() const;
 
+  void UpdateControlInfos();
+
   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);
@@ -57,6 +59,7 @@ class Channels {
 
   bool XuCamCtrlQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
   std::int32_t XuCamCtrlGet(Option option) const;
+  void XuCamCtrlSet(Option option, std::int32_t value) const;
 
   control_info_t PuControlInfo(Option option) const;
   control_info_t XuControlInfo(Option option) const;