Add imu range setting

doc/zh-Hans/spec_control_api.md

@@ -26,3 +26,5 @@
 | HDR 模式 | hdr_mode | 1 | 0 | 0 | 1 | √ | 0x1F | XU_CAM_CTRL | 0：10-bit；1：12-bit |
 | 零漂标定 | zero_drift_calibration | | - | - | - | × | - | XU_HALF_DUPLEX | |
 | 擦除芯片 | erase_chip | | - | - | - | × | - | XU_HALF_DUPLEX | |
+| 加速度计量程 | accelerometer_range | 2 | 12 | 6 | 48 | √ | - | XU_CAM_CTRL | 0x0100 | |
+| 陀螺仪量程 | gyroscope_range | 2 | 1000 | 250 | 4000 | √ | - | XU_CAM_CTRL | 0x0100 | |

include/mynteye/types.h

@@ -195,6 +195,18 @@ enum class Option : std::uint8_t {
   ZERO_DRIFT_CALIBRATION,
   /** Erase chip */
   ERASE_CHIP,
+  /**
+   * The range of accelerometer
+   *
+   *   values: {4,8,16,32}, default: 8
+   */
+  ACCELEROMETER_RANGE,
+  /**
+   * The range of gyroscope
+   *
+   *   values: {500,1000,2000,4000}, default: 1000
+   */
+  GYROSCOPE_RANGE,
   /** Last guard */
   LAST
 };

samples/tutorials/CMakeLists.txt

@@ -114,6 +114,7 @@ make_executable2(get_with_plugin SRCS data/get_with_plugin.cc WITH_OPENCV)
 ## control
 
 make_executable2(ctrl_framerate SRCS control/framerate.cc WITH_OPENCV)
+make_executable2(ctrl_imu_range SRCS control/imu_range.cc WITH_OPENCV)
 make_executable2(ctrl_auto_exposure
   SRCS control/auto_exposure.cc util/cv_painter.cc
   WITH_OPENCV

samples/tutorials/control/imu_range.cc

@@ -0,0 +1,89 @@
+// 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 <atomic>
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include "mynteye/logger.h"
+#include "mynteye/api/api.h"
+#include "mynteye/util/times.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+  if (!api)
+    return 1;
+
+  // ACCELEROMETER_RANGE values: 4, 8, 16, 32
+  api->SetOptionValue(Option::ACCELEROMETER_RANGE, 8);
+  // GYROSCOPE_RANGE values: 500, 1000, 2000, 4000
+  api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000);
+
+  LOG(INFO) << "Set ACCELEROMETER_RANGE to "
+            << api->GetOptionValue(Option::ACCELEROMETER_RANGE);
+  LOG(INFO) << "Set GYROSCOPE_RANGE to "
+            << api->GetOptionValue(Option::GYROSCOPE_RANGE);
+
+  // Count img
+  std::atomic_uint img_count(0);
+  api->SetStreamCallback(
+      Stream::LEFT, [&img_count](const api::StreamData &data) {
+        CHECK_NOTNULL(data.img);
+        ++img_count;
+      });
+
+  // Count imu
+  std::atomic_uint imu_count(0);
+  api->SetMotionCallback([&imu_count](const api::MotionData &data) {
+    CHECK_NOTNULL(data.imu);
+    ++imu_count;
+  });
+
+  api->Start(Source::ALL);
+
+  cv::namedWindow("frame");
+
+  auto &&time_beg = times::now();
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+    cv::imshow("frame", img);
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+  auto &&time_end = times::now();
+
+  api->Stop(Source::ALL);
+
+  // Calculate img fps and imu hz
+  float elapsed_ms =
+      times::count<times::microseconds>(time_end - time_beg) * 0.001f;
+  LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
+            << ", end: " << times::to_local_string(time_end)
+            << ", cost: " << elapsed_ms << "ms";
+  LOG(INFO) << "Img count: " << img_count
+            << ", fps: " << (1000.f * img_count / elapsed_ms);
+  LOG(INFO) << "Imu count: " << imu_count
+            << ", hz: " << (1000.f * imu_count / elapsed_ms);
+  return 0;
+}

src/mynteye/device/channels.cc

@@ -63,6 +63,12 @@ int XuCamCtrlId(Option option) {
     case Option::FRAME_RATE:
       return 7;
       break;
+    case Option::ACCELEROMETER_RANGE:
+      return 9;
+      break;
+    case Option::GYROSCOPE_RANGE:
+      return 10;
+      break;
     default:
       LOG(FATAL) << "No cam ctrl id for " << option;
   }
@@ -135,7 +141,8 @@ 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::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE}) {
     control_infos_[option] = XuControlInfo(option);
   }
 
@@ -177,6 +184,8 @@ std::int32_t Channels::GetControlValue(const Option &option) const {
     case Option::DESIRED_BRIGHTNESS:
     case Option::IR_CONTROL:
     case Option::HDR_MODE:
+    case Option::ACCELEROMETER_RANGE:
+    case Option::GYROSCOPE_RANGE:
       return XuCamCtrlGet(option);
     case Option::ZERO_DRIFT_CALIBRATION:
     case Option::ERASE_CHIP:
@@ -232,6 +241,16 @@ void Channels::SetControlValue(const Option &option, std::int32_t value) {
         break;
       XuCamCtrlSet(option, value);
     } break;
+    case Option::ACCELEROMETER_RANGE: {
+      if (!in_range() || !in_values({4, 8, 16, 32}))
+        break;
+      XuCamCtrlSet(option, value);
+    } break;
+    case Option::GYROSCOPE_RANGE: {
+      if (!in_range() || !in_values({500, 1000, 2000, 4000}))
+        break;
+      XuCamCtrlSet(option, value);
+    } break;
     case Option::EXPOSURE_MODE:
     case Option::MAX_GAIN:
     case Option::MAX_EXPOSURE_TIME:
@@ -268,6 +287,8 @@ bool Channels::RunControlAction(const Option &option) const {
     case Option::DESIRED_BRIGHTNESS:
     case Option::IR_CONTROL:
     case Option::HDR_MODE:
+    case Option::ACCELEROMETER_RANGE:
+    case Option::GYROSCOPE_RANGE:
       LOG(WARNING) << option << " run action useless";
       return false;
     default:

src/mynteye/device/config.cc

@@ -26,7 +26,8 @@ const std::map<Model, OptionSupports> option_supports_map = {
      {Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST, 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::ZERO_DRIFT_CALIBRATION, Option::ERASE_CHIP}}};
+      Option::HDR_MODE, Option::ZERO_DRIFT_CALIBRATION, Option::ERASE_CHIP,
+      Option::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE}}};
 
 const std::map<Model, std::map<Capabilities, StreamRequests>>
     stream_requests_map = {

src/mynteye/device/motions.cc

@@ -34,6 +34,14 @@ Motions::~Motions() {
 void Motions::SetMotionCallback(motion_callback_t callback) {
   motion_callback_ = callback;
   if (motion_callback_) {
+    accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE);
+    if (accel_range == -1)
+      accel_range = 8;
+
+    gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE);
+    if (gyro_range == -1)
+      gyro_range = 1000;
+
     channels_->SetImuCallback([this](const ImuPacket &packet) {
       if (!motion_callback_ && !motion_datas_enabled_) {
         return;
@@ -46,12 +54,12 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
         //   LOG(WARNING) << "Imu timestamp offset is incorrect";
         // }
         imu->timestamp = packet.timestamp + seg.offset;
-        imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
+        imu->accel[0] = seg.accel[0] * 1.f * accel_range / 0x10000;
-        imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
+        imu->accel[1] = seg.accel[1] * 1.f * accel_range / 0x10000;
-        imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
+        imu->accel[2] = seg.accel[2] * 1.f * accel_range / 0x10000;
-        imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
+        imu->gyro[0] = seg.gyro[0] * 1.f * gyro_range / 0x10000;
-        imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
+        imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
-        imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
+        imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
         imu->temperature = seg.temperature / 326.8f + 25;
 
         std::lock_guard<std::mutex> _(mtx_datas_);

src/mynteye/device/motions.h

@@ -57,6 +57,9 @@ class Motions {
   bool is_imu_tracking;
 
   std::mutex mtx_datas_;
+
+  int accel_range = 8;
+  int gyro_range = 1000;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/types.cc

@@ -114,6 +114,8 @@ const char *to_string(const Option &value) {
     CASE(HDR_MODE)
     CASE(ZERO_DRIFT_CALIBRATION)
     CASE(ERASE_CHIP)
+    CASE(ACCELEROMETER_RANGE)
+    CASE(GYROSCOPE_RANGE)
     default:
       CHECK(is_valid(value));
       return "Option::UNKNOWN";