From 0d1b0639426987c04003897e2be31421bb6ff27f Mon Sep 17 00:00:00 2001
From: Kalman <kalmanxiong@slightech.com>
Date: Thu, 16 Aug 2018 19:34:01 +0800
Subject: [PATCH] Add options

---
 include/mynteye/types.h  | 30 ++++++++++++++++++++++++++++++
 src/internal/channels.cc | 37 +++++++++++++++++++++++++++++++++++--
 src/internal/config.cc   |  7 ++++++-
 src/internal/motions.cc  | 14 ++++++++------
 src/internal/motions.h   |  3 +++
 5 files changed, 82 insertions(+), 9 deletions(-)

diff --git a/include/mynteye/types.h b/include/mynteye/types.h
index 706bc44..117a2a4 100644
--- a/include/mynteye/types.h
+++ b/include/mynteye/types.h
@@ -197,6 +197,36 @@ enum class Option : std::uint8_t {
   ZERO_DRIFT_CALIBRATION,
   /** Erase chip */
   ERASE_CHIP,
+  /**
+   * min exposure time, valid if auto-exposure
+   *
+   *   range: [0,?], default: ?
+   */
+  MIN_EXPOSURE_TIME,
+  /**
+   * The range of accelerometer
+   *
+   *   values: {?}, default: ?
+   */
+  ACCELEROMETER_RANGE,
+  /**
+   * The range of gyroscope
+   *
+   *   values: {?}, default: ?
+   */
+  GYROSCOPE_RANGE,
+  /**
+   * The parameter of accelerometer low pass filter
+   *
+   *   values: {?}, default: ?
+   */
+  ACCELEROMETER_LOW_PASS_FILTER,
+  /**
+   * The parameter of gyroscope low pass filter
+   *
+   *   values: {?}, default: ?
+   */
+  GYROSCOPE_LOW_PASS_FILTER,
   /** Last guard */
   LAST
 };
diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index 58c6668..3426a6e 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -64,6 +64,21 @@ int XuCamCtrlId(Option option) {
     case Option::FRAME_RATE:
       return 7;
       break;
+    case Option::MIN_EXPOSURE_TIME:
+      return 8;
+      break;
+    case Option::ACCELEROMETER_RANGE:
+      return 9;
+      break;
+    case Option::GYROSCOPE_RANGE:
+      return 10;
+      break;
+    case Option::ACCELEROMETER_LOW_PASS_FILTER:
+      return 11;
+      break;
+    case Option::GYROSCOPE_LOW_PASS_FILTER:
+      return 12;
+      break;
     default:
       LOG(FATAL) << "No cam ctrl id for " << option;
   }
@@ -137,7 +152,10 @@ void Channels::UpdateControlInfos() {
     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})
+             Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL, Option::HDR_MODE
+             Option::MIN_EXPOSURE_TIME, Option::ACCELEROMETER_RANGE,
+             Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
+             Option::GYROSCOPE_LOW_PASS_FILTER})
     {
        control_infos_[option] = XuControlInfo(option);
     }
@@ -182,6 +200,11 @@ std::int32_t Channels::GetControlValue(const Option &option) const {
     case Option::DESIRED_BRIGHTNESS:
     case Option::IR_CONTROL:
     case Option::HDR_MODE:
+    case Option::MIN_EXPOSURE_TIME:
+    case Option::ACCELEROMETER_RANGE:
+    case Option::GYROSCOPE_RANGE:
+    case Option::ACCELEROMETER_LOW_PASS_FILTER:
+    case Option::GYROSCOPE_LOW_PASS_FILTER:
       return XuCamCtrlGet(option);
     case Option::ZERO_DRIFT_CALIBRATION:
     case Option::ERASE_CHIP:
@@ -242,7 +265,12 @@ void Channels::SetControlValue(const Option &option, std::int32_t value) {
     case Option::MAX_EXPOSURE_TIME:
     case Option::DESIRED_BRIGHTNESS:
     case Option::IR_CONTROL:
-    case Option::HDR_MODE: {
+    case Option::HDR_MODE:
+    case Option::MIN_EXPOSURE_TIME:
+    case Option::ACCELEROMETER_RANGE:
+    case Option::GYROSCOPE_RANGE:
+    case Option::ACCELEROMETER_LOW_PASS_FILTER:
+    case Option::GYROSCOPE_LOW_PASS_FILTER: {
       if (!in_range())
         break;
       XuCamCtrlSet(option, value);
@@ -273,6 +301,11 @@ bool Channels::RunControlAction(const Option &option) const {
     case Option::DESIRED_BRIGHTNESS:
     case Option::IR_CONTROL:
     case Option::HDR_MODE:
+    case Option::MIN_EXPOSURE_TIME:
+    case Option::ACCELEROMETER_RANGE:
+    case Option::GYROSCOPE_RANGE:
+    case Option::ACCELEROMETER_LOW_PASS_FILTER:
+    case Option::GYROSCOPE_LOW_PASS_FILTER:
       LOG(WARNING) << option << " run action useless";
       return false;
     default:
diff --git a/src/internal/config.cc b/src/internal/config.cc
index ca4eb26..40df322 100644
--- a/src/internal/config.cc
+++ b/src/internal/config.cc
@@ -22,7 +22,12 @@ const std::map<Model, CapabilitiesSupports> capabilities_supports_map = {
     {Model::STANDARD, {Capabilities::STEREO_COLOR, Capabilities::IMU}}};
 
 const std::map<Model, OptionSupports> option_supports_map = {
-    {Model::STANDARD, {Option::ERASE_CHIP}}};
+    {Model::STANDARD,
+     {Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST, Option::EXPOSURE_MODE,
+      Option::MAX_EXPOSURE_TIME, Option::ERASE_CHIP, Option::MIN_EXPOSURE_TIME,
+      Option::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE,
+      Option::ACCELEROMETER_LOW_PASS_FILTER,
+      Option::GYROSCOPE_LOW_PASS_FILTER}}};
 
 const std::map<Model, std::map<Capabilities, StreamRequests>>
     stream_requests_map = {
diff --git a/src/internal/motions.cc b/src/internal/motions.cc
index 871cdde..7b2ad52 100644
--- a/src/internal/motions.cc
+++ b/src/internal/motions.cc
@@ -35,6 +35,8 @@ Motions::~Motions() {
 void Motions::SetMotionCallback(motion_callback_t callback) {
   motion_callback_ = callback;
   if (motion_callback_) {
+    accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE);
+    gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE);
     channels_->SetImuCallback([this](const ImuPacket &packet) {
       if (!motion_callback_ && !motion_datas_enabled_) {
         LOG(WARNING) << "";
@@ -53,9 +55,9 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
         imu->temperature = seg.temperature / 326.8f + 25;
 
         if (imu->flag == 1) {
-          imu->accel[0] = seg.aceel_or_gyro[0] * 12.f / 0x10000;
-          imu->accel[1] = seg.aceel_or_gyro[1] * 12.f / 0x10000;
-          imu->accel[2] = seg.aceel_or_gyro[2] * 12.f / 0x10000;
+          imu->accel[0] = seg.aceel_or_gyro[0] * 1.f * accel_range / 0x10000;
+          imu->accel[1] = seg.aceel_or_gyro[1] * 1.f * accel_range / 0x10000;
+          imu->accel[2] = seg.aceel_or_gyro[2] * 1.f * accel_range / 0x10000;
           imu->gyro[0] = 0;
           imu->gyro[1] = 0;
           imu->gyro[2] = 0;
@@ -63,9 +65,9 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
           imu->accel[0] = 0;
           imu->accel[1] = 0;
           imu->accel[2] = 0;
-          imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000;
-          imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000;
-          imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000;
+          imu->gyro[0] = seg.aceel_or_gyro[0] * 1.f * gyro_range / 0x10000;
+          imu->gyro[1] = seg.aceel_or_gyro[1] * 1.f * gyro_range / 0x10000;
+          imu->gyro[2] = seg.aceel_or_gyro[2] * 1.f * gyro_range / 0x10000;
         } else {
           imu->Reset();
         }
diff --git a/src/internal/motions.h b/src/internal/motions.h
index 0472605..fb9e671 100644
--- a/src/internal/motions.h
+++ b/src/internal/motions.h
@@ -57,6 +57,9 @@ class Motions {
   bool is_imu_tracking;
 
   std::mutex mtx_datas_;
+
+  int accel_range = 12;
+  int gyro_range = 1000;
 };
 
 MYNTEYE_END_NAMESPACE