Add imu tracking

samples/device/camera.cc

@@ -64,26 +64,40 @@ int main(int argc, char *argv[]) {
   std::size_t left_count = 0;
   device->SetStreamCallback(
       Stream::LEFT, [&left_count](const device::StreamData &data) {
+        CHECK_NOTNULL(data.img);
         ++left_count;
         VLOG(2) << Stream::LEFT << ", count: " << left_count;
-        if (data.img) {
-          VLOG(2) << "  frame_id: " << data.img->frame_id
-                  << ", timestamp: " << data.img->timestamp
-                  << ", exposure_time: " << data.img->exposure_time;
-        }
+        VLOG(2) << "  frame_id: " << data.img->frame_id
+                << ", timestamp: " << data.img->timestamp
+                << ", exposure_time: " << data.img->exposure_time;
       });
   std::size_t right_count = 0;
   device->SetStreamCallback(
       Stream::RIGHT, [&right_count](const device::StreamData &data) {
+        CHECK_NOTNULL(data.img);
         ++right_count;
         VLOG(2) << Stream::RIGHT << ", count: " << right_count;
-        if (data.img) {
-          VLOG(2) << "  frame_id: " << data.img->frame_id
-                  << ", timestamp: " << data.img->timestamp
-                  << ", exposure_time: " << data.img->exposure_time;
-        }
+        VLOG(2) << "  frame_id: " << data.img->frame_id
+                << ", timestamp: " << data.img->timestamp
+                << ", exposure_time: " << data.img->exposure_time;
       });
 
+  std::size_t imu_count = 0;
+  device->SetMotionCallback([&imu_count](const device::MotionData &data) {
+    CHECK_NOTNULL(data.imu);
+    ++imu_count;
+    LOG(INFO) << "Imu count: " << imu_count;
+    LOG(INFO) << "  frame_id: " << data.imu->frame_id
+              << ", timestamp: " << data.imu->timestamp
+              << ", accel_x: " << data.imu->accel[0]
+              << ", accel_y: " << data.imu->accel[1]
+              << ", accel_z: " << data.imu->accel[2]
+              << ", gyro_x: " << data.imu->gyro[0]
+              << ", gyro_y: " << data.imu->gyro[1]
+              << ", gyro_z: " << data.imu->gyro[2]
+              << ", temperature: " << data.imu->temperature;
+  });
+
   device->Start(Source::ALL);
 
   cv::namedWindow("frame");

src/device/device.cc

@@ -324,7 +324,27 @@ void Device::StartMotionTracking() {
     LOG(WARNING) << "Cannot start motion tracking without first stopping it";
     return;
   }
-  // TODO(JohnZhao)
+  channels_->StartImuTracking([this](const ImuPacket &packet) {
+    if (!HasMotionCallback())
+      return;
+    for (auto &&seg : packet.segments) {
+      auto &&imu = std::make_shared<ImuData>();
+      imu->frame_id = seg.frame_id;
+      if (seg.offset < 0 &&
+          static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
+        LOG(WARNING) << "Imu timestamp offset is incorrect";
+      }
+      imu->timestamp = packet.timestamp + seg.offset;
+      imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
+      imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
+      imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
+      imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
+      imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
+      imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
+      imu->temperature = seg.temperature / 326.8f + 25;
+      motion_callback_({imu});
+    }
+  });
   motion_tracking_ = true;
 }
 
@@ -333,7 +353,7 @@ void Device::StopMotionTracking() {
     LOG(WARNING) << "Cannot stop motion tracking without first starting it";
     return;
   }
-  // TODO(JohnZhao)
+  channels_->StopImuTracking();
   motion_tracking_ = false;
 }
 

src/internal/channels.cc

@@ -2,6 +2,8 @@
 
 #include <glog/logging.h>
 
+#include <chrono>
+#include <iomanip>
 #include <stdexcept>
 
 MYNTEYE_BEGIN_NAMESPACE
@@ -54,13 +56,19 @@ int XuHalfDuplexId(Option option) {
 
 }  // namespace
 
-Channels::Channels(std::shared_ptr<uvc::device> device) : device_(device) {
+Channels::Channels(std::shared_ptr<uvc::device> device)
+    : device_(device),
+      is_imu_tracking_(false),
+      imu_track_stop_(false),
+      imu_sn_(0),
+      imu_callback_(nullptr) {
   VLOG(2) << __func__;
   UpdateControlInfos();
 }
 
 Channels::~Channels() {
   VLOG(2) << __func__;
+  StopImuTracking();
 }
 
 void Channels::LogControlInfos() const {
@@ -198,6 +206,59 @@ bool Channels::RunControlAction(const Option &option) const {
   }
 }
 
+void Channels::SetImuCallback(imu_callback_t callback) {
+  imu_callback_ = callback;
+}
+
+void Channels::StartImuTracking(imu_callback_t callback) {
+  if (is_imu_tracking_) {
+    LOG(WARNING) << "start imu tracking failed, is tracking already";
+    return;
+  }
+  if (callback) {
+    imu_callback_ = callback;
+  }
+  is_imu_tracking_ = true;
+  imu_track_thread_ = std::thread([this]() {
+    imu_sn_ = 0;
+    ImuReqPacket req_packet{imu_sn_};
+    ImuResPacket res_packet;
+    auto sleep_milli = [](std::intmax_t n) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(n));
+    };
+    while (!imu_track_stop_) {
+      req_packet.serial_number = imu_sn_;
+      if (!XuImuWrite(req_packet)) {
+        sleep_milli(5);
+        continue;
+      }
+
+      if (!XuImuRead(&res_packet)) {
+        sleep_milli(5);
+        continue;
+      }
+
+      imu_sn_ = res_packet.packet.serial_number;
+
+      if (imu_callback_) {
+        imu_callback_(res_packet.packet);
+      }
+    }
+  });
+}
+
+void Channels::StopImuTracking() {
+  if (!is_imu_tracking_) {
+    return;
+  }
+  if (imu_track_thread_.joinable()) {
+    imu_track_stop_ = true;
+    imu_track_thread_.join();
+    imu_track_stop_ = false;
+    is_imu_tracking_ = false;
+  }
+}
+
 bool Channels::PuControlRange(
     Option option, int32_t *min, int32_t *max, int32_t *def) const {
   CHECK_NOTNULL(device_);
@@ -291,8 +352,36 @@ bool Channels::XuImuRead(ImuResPacket *res) const {
   static std::uint8_t data[2000]{};
   // std::fill(data, data + 2000, 0);  // reset
   if (XuControlQuery(CHANNEL_IMU_READ, uvc::XU_QUERY_GET, 2000, data)) {
-    VLOG(2) << "XuImuRead response success";
     res->from_data(data);
+
+    if (res->header != 0x5B) {
+      LOG(WARNING) << "Imu response packet header must be 0x5B, but 0x"
+                   << std::hex << std::uppercase << std::setw(2)
+                   << std::setfill('0') << static_cast<int>(res->header)
+                   << " now";
+      return false;
+    }
+
+    if (res->state != 0) {
+      LOG(WARNING) << "Imu response packet state must be 0, but " << res->state
+                   << " now";
+      return false;
+    }
+
+    std::uint8_t checksum = 0;
+    for (std::size_t i = 4, n = 4 + res->size; i < n; i++) {
+      checksum = (checksum ^ data[i]);
+    }
+    if (checksum != res->checksum) {
+      LOG(WARNING) << "Imu response packet checksum should be 0x" << std::hex
+                   << std::uppercase << std::setw(2) << std::setfill('0')
+                   << static_cast<int>(res->checksum) << ", but 0x"
+                   << std::setw(2) << std::setfill('0')
+                   << static_cast<int>(res->checksum) << " now";
+      return false;
+    }
+
+    VLOG(2) << "XuImuRead response success";
     return true;
   } else {
     LOG(WARNING) << "XuImuRead response failed";

src/internal/channels.h

@@ -4,6 +4,7 @@
 
 #include <map>
 #include <memory>
+#include <thread>
 
 #include "mynteye/mynteye.h"
 
@@ -42,6 +43,8 @@ class Channels {
     XU_CMD_LAST
   } xu_cmd_t;
 
+  using imu_callback_t = std::function<void(const ImuPacket &packet)>;
+
   explicit Channels(std::shared_ptr<uvc::device> device);
   ~Channels();
 
@@ -54,6 +57,10 @@ class Channels {
 
   bool RunControlAction(const Option &option) const;
 
+  void SetImuCallback(imu_callback_t callback);
+  void StartImuTracking(imu_callback_t callback = nullptr);
+  void StopImuTracking();
+
  private:
   bool PuControlRange(
       Option option, int32_t *min, int32_t *max, int32_t *def) const;
@@ -81,6 +88,13 @@ class Channels {
   std::shared_ptr<uvc::device> device_;
 
   std::map<Option, control_info_t> control_infos_;
+
+  bool is_imu_tracking_;
+  std::thread imu_track_thread_;
+  volatile bool imu_track_stop_;
+
+  std::uint32_t imu_sn_;
+  imu_callback_t imu_callback_;
 };
 
 MYNTEYE_END_NAMESPACE