feat(correspondence): support keep imu sequence

src/mynteye/api/api.cc

@@ -518,9 +518,11 @@ std::vector<api::MotionData> API::GetMotionDatas() {
   }
 }
 
-void API::EnableTimestampCorrespondence(const Stream &stream) {
+void API::EnableTimestampCorrespondence(const Stream &stream,
+    bool keep_accel_then_gyro) {
   if (correspondence_ == nullptr) {
     correspondence_.reset(new Correspondence(device_, stream));
+    correspondence_->KeepAccelThenGyro(keep_accel_then_gyro);
     {
       device_->DisableMotionDatas();
       if (callback_) {

src/mynteye/api/correspondence.cc

@@ -16,11 +16,15 @@
 #include "mynteye/device/device.h"
 #include "mynteye/logger.h"
 
+#define MYNTEYE_IMU_SEQ_FIRST  1  // accel
+#define MYNTEYE_IMU_SEQ_SECOND 2  // gyro
+
 MYNTEYE_BEGIN_NAMESPACE
 
 Correspondence::Correspondence(const std::shared_ptr<Device> &device,
     const Stream &stream)
-  : device_(device), stream_(stream), ready_image_timestamp_(0) {
+  : device_(device), stream_(stream), ready_image_timestamp_(0),
+    keep_accel_then_gyro_(false) {
   VLOG(2) << __func__;
   // set matched stream to be watched too,
   // aim to make stream and matched stream correspondence
@@ -54,6 +58,10 @@ bool Correspondence::Watch(const Stream &stream) const {
   return false;
 }
 
+void Correspondence::KeepAccelThenGyro(bool enabled) {
+  keep_accel_then_gyro_ = enabled;
+}
+
 void Correspondence::OnStreamDataCallback(
     const Stream &stream, const api::StreamData &data) {
   if (!Watch(stream)) {
@@ -143,7 +151,11 @@ std::vector<api::StreamData> Correspondence::GetStreamDatas(
 }
 
 std::vector<api::MotionData> Correspondence::GetMotionDatas() {
-  return GetReadyMotionDatas();
+  auto &&datas = GetReadyMotionDatas();
+  if (keep_accel_then_gyro_ && device_->GetModel() != Model::STANDARD) {
+    KeepAccelThenGyro(datas);  // only s2 need do this
+  }
+  return datas;
 }
 
 void Correspondence::EnableStreamMatch() {
@@ -274,4 +286,46 @@ std::vector<api::MotionData> Correspondence::GetReadyMotionDatas() {
   return result;
 }
 
+void Correspondence::KeepAccelThenGyro(std::vector<api::MotionData> &datas) {
+  auto n = datas.size();
+  if (n == 0) return;
+
+  static std::shared_ptr<ImuData> last_imu = nullptr;
+
+  std::uint8_t prev_flag = 0;
+  for (auto it = datas.begin(); it != datas.end(); ) {
+    auto flag = it->imu->flag;
+    if (flag == 0) {
+      ++it;  // unexpected
+      continue;
+    }
+
+    bool is_first = (it == datas.begin());
+    bool is_last = (it == datas.end() - 1);
+    bool ok = false;
+    if (is_first) {
+      ok = (flag == MYNTEYE_IMU_SEQ_FIRST);
+    } else {
+      if (flag == MYNTEYE_IMU_SEQ_FIRST) {
+        ok = (prev_flag == MYNTEYE_IMU_SEQ_SECOND);
+      } else if (flag == MYNTEYE_IMU_SEQ_SECOND) {
+        ok = (prev_flag == MYNTEYE_IMU_SEQ_FIRST);
+      }
+    }
+    if (ok && is_last) {
+      ok = (flag == MYNTEYE_IMU_SEQ_SECOND);
+    }
+
+    if (ok) {
+      prev_flag = flag;
+      if (is_last) {
+        last_imu = nullptr;
+      }
+      ++it;
+    } else {
+      it = datas.erase(it);
+    }
+  }
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/api/correspondence.h

@@ -32,6 +32,7 @@ class Correspondence {
   ~Correspondence();
 
   bool Watch(const Stream &stream) const;
+  void KeepAccelThenGyro(bool enabled);
 
   void OnStreamDataCallback(const Stream &stream, const api::StreamData &data);
   void OnMotionDataCallback(const device::MotionData &data);
@@ -55,6 +56,8 @@ class Correspondence {
   std::vector<api::StreamData> GetReadyStreamData(bool matched);
   std::vector<api::MotionData> GetReadyMotionDatas();
 
+  void KeepAccelThenGyro(std::vector<api::MotionData> &datas);  // NOLINT
+
   std::shared_ptr<Device> device_;
   Stream stream_;
   Stream stream_match_;
@@ -73,6 +76,8 @@ class Correspondence {
   std::condition_variable_any cond_stream_datas_;
 
   std::uint64_t ready_image_timestamp_;
+
+  bool keep_accel_then_gyro_;
 };
 
 MYNTEYE_END_NAMESPACE