refactor(channel):init channel need model

src/mynteye/device/channels.cc

@@ -120,13 +120,15 @@ void CheckSpecVersion(const Version *spec_version) {
 
 }  // namespace
 
-Channels::Channels(std::shared_ptr<uvc::device> device)
+Channels::Channels(const Model &model, std::shared_ptr<uvc::device> device)
     : device_(device),
+      model_(model),
       is_imu_tracking_(false),
       imu_track_stop_(false),
       imu_sn_(0),
       imu_callback_(nullptr) {
   VLOG(2) << __func__;
+  imu_res_version_ = (model == Model::STANDARD) ? 1 : 2;
   // UpdateControlInfos();
 }
 
@@ -327,13 +329,17 @@ bool Channels::RunControlAction(const Option &option) const {
   }
 }
 
+std::uint8_t Channels::GetImuResVersion() {
+  return imu_res_version_;
+}
+
 void Channels::SetImuCallback(imu_callback_t callback) {
   imu_callback_ = callback;
 }
 
 void Channels::DoImuTrack() {
   static ImuReqPacket req_packet{0};
-  static ImuResPacket res_packet(model_);
+  static ImuResPacket res_packet(imu_res_version_);
 
   req_packet.serial_number = imu_sn_;
   if (!XuImuWrite(req_packet)) {

src/mynteye/device/channels.h

@@ -71,9 +71,8 @@ class MYNTEYE_API Channels {
   using img_params_t = std::map<Resolution, device::img_params_t>;
   using imu_params_t = device::imu_params_t;
 
-  std::uint8_t model_;
+  explicit Channels(
-
+      const Model &model, std::shared_ptr<uvc::device> device);
-  explicit Channels(std::shared_ptr<uvc::device> device);
   ~Channels();
 
   void LogControlInfos() const;
@@ -85,6 +84,8 @@ class MYNTEYE_API Channels {
 
   bool RunControlAction(const Option &option) const;
 
+  std::uint8_t GetImuResVersion();
+
   void SetImuCallback(imu_callback_t callback);
   void DoImuTrack();
 
@@ -131,6 +132,10 @@ class MYNTEYE_API Channels {
   control_info_t PuControlInfo(Option option) const;
   control_info_t XuControlInfo(Option option) const;
 
+  std::uint8_t imu_res_version_;
+
+  Model model_;
+
   std::shared_ptr<uvc::device> device_;
 
   std::map<Option, control_info_t> control_infos_;

src/mynteye/device/device.cc

@@ -85,7 +85,7 @@ Device::Device(const Model &model, std::shared_ptr<uvc::device> device)
       model_(model),
       device_(device),
       streams_(nullptr),
-      channels_(std::make_shared<Channels>(device)),
+      channels_(std::make_shared<Channels>(model_, device)),
       motions_(std::make_shared<Motions>(channels_)) {
   VLOG(2) << __func__;
   ReadAllInfos();

src/mynteye/device/motions.cc

@@ -36,7 +36,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
   if (motion_callback_) {
     accel_range = channels_->GetControlValue(Option::ACCELEROMETER_RANGE);
     if (accel_range == -1)
-      accel_range = (channels_->model_) ? 8 : 12;  // ugly
+      accel_range = (channels_->GetImuResVersion() == 1) ? 8 : 12;
 
     gyro_range = channels_->GetControlValue(Option::GYROSCOPE_RANGE);
     if (gyro_range == -1)
@@ -78,8 +78,7 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
   }
 }
 
-void Motions::DoMotionTrack(std::uint8_t model) {
+void Motions::DoMotionTrack() {
-  channels_->model_ = model;
   channels_->DoImuTrack();
 }
 

src/mynteye/device/motions.h

@@ -37,7 +37,7 @@ class Motions {
   ~Motions();
 
   void SetMotionCallback(motion_callback_t callback);
-  void DoMotionTrack(std::uint8_t model);
+  void DoMotionTrack();
 
   void StartMotionTracking();
   void StopMotionTracking();
@@ -58,8 +58,8 @@ class Motions {
 
   std::mutex mtx_datas_;
 
-  int accel_range = 12;
+  int accel_range;
-  int gyro_range = 1000;
+  int gyro_range;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/device/standard/device_s.cc

@@ -44,7 +44,7 @@ void StandardDevice::StartVideoStreaming() {
 void StandardDevice::OnStereoStreamUpdate() {
   if (motion_tracking_) {
     auto &&motions = this->motions();
-    motions->DoMotionTrack(1);
+    motions->DoMotionTrack();
   }
 }
 

src/mynteye/device/standard2/device_s2.cc

@@ -39,7 +39,7 @@ std::shared_ptr<StreamsAdapter> Standard2Device::CreateStreamsAdapter() const {
 void Standard2Device::OnStereoStreamUpdate() {
   if (motion_tracking_) {
     auto &&motions = this->motions();
-    motions->DoMotionTrack(2);
+    motions->DoMotionTrack();
   }
 }
 

src/mynteye/device/types.h

@@ -322,9 +322,9 @@ struct ImuSegment {
     gyro[2] = (flag == 2) ? group.accel_or_gyro[2] : 0;
   }
 
-  explicit ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) {
+  ImuSegment(std::uint32_t timestamp, ImuGroupS1 group) {
     frame_id = static_cast<uint32_t> (group.frame_id);
-    this->timestamp = static_cast<uint64_t> (timestamp + group.offset);
+    this->timestamp = static_cast<uint64_t> (timestamp + group.offset) * 10;
     flag = 3;
     temperature = group.temperature;
     accel[0] = group.accel[0];
@@ -343,19 +343,19 @@ struct ImuSegment {
  */
 #pragma pack(push, 1)
 struct ImuPacket {
-  std::uint8_t model;
+  std::uint8_t version;
   std::uint8_t count;
   std::uint32_t serial_number;
   std::vector<ImuSegment> segments;
 
   ImuPacket() = default;
   explicit ImuPacket(
-      std::uint8_t model, std::uint8_t seg_count, std::uint8_t *data)
+      std::uint8_t version, std::uint8_t seg_count, std::uint8_t *data)
-        : model(model), count(seg_count) {
+        : version(version), count(seg_count) {
           from_data(data);
         }
   void from_data(std::uint8_t *data) {
-        if (model == 1) {
+        if (version == 1) {
           serial_number =
               (*(data) << 24) | (*(data + 1) << 16) |
               (*(data + 2) << 8) | *(data + 3);
@@ -369,7 +369,7 @@ struct ImuPacket {
             ImuGroupS1 group(data + 9 + (seg_n * i));
             segments.push_back(ImuSegment(timestamp, group));
           }
-        } else if (model == 2) {
+        } else if (version == 2) {
           std::size_t seg_n = sizeof(ImuGroupS2);  // 21
           for (std::size_t i = 0; i < count; i++) {
             ImuGroupS2 group(data + seg_n * i);
@@ -387,7 +387,7 @@ struct ImuPacket {
  */
 #pragma pack(push, 1)
 struct ImuResPacket {
-  std::uint8_t model;
+  std::uint8_t version;
   std::uint8_t header;
   std::uint8_t state;
   std::uint16_t size;
@@ -395,27 +395,27 @@ struct ImuResPacket {
   std::uint8_t checksum;
 
   ImuResPacket() = default;
-  explicit ImuResPacket(std::uint8_t model) : model(model) {}
+  explicit ImuResPacket(std::uint8_t version) : version(version) {}
 
   void from_data(std::uint8_t *data) {
     header = *data;
     state = *(data + 1);
     size = (*(data + 2) << 8) | *(data + 3);
 
-    if (model == 1) {
+    if (version == 1) {
       std::size_t seg_n = sizeof(ImuGroupS1);  // 18
       for (std::size_t i = 4; i < size;) {
-        ImuPacket packet(model, 0, data + i);
+        ImuPacket packet(version, 0, data + i);
         packets.push_back(packet);
         i += 9 + (packet.count * seg_n);
       }
       checksum = *(data + 4 + size);
     }
 
-    if (model == 2) {
+    if (version == 2) {
       std::size_t seg_n = sizeof(ImuGroupS2);  // 21
       std::uint8_t seg_count = size / seg_n;
-      ImuPacket packet(model, seg_count, data + 4);
+      ImuPacket packet(version, seg_count, data + 4);
       packets.push_back(packet);
       // packet(2);
       checksum = *(data + 4 + size);