change the data protocol of imu

include/mynteye/types.h

@@ -441,10 +441,10 @@ struct MYNTEYE_API ImgData {
  * IMU data.
  */
 struct MYNTEYE_API ImuData {
-  /** Image frame id */
+  /** accel or gyro flag:1 for accel,2 for gyro,3 for both */
-  std::uint16_t frame_id;
+  std::uint8_t flag;
   /** IMU timestamp in 0.01ms */
-  std::uint32_t timestamp;
+  std::uint64_t timestamp;
   /** IMU accelerometer data for 3-axis: X, Y, Z. */
   double accel[3];
   /** IMU gyroscope data for 3-axis: X, Y, Z. */
@@ -453,7 +453,7 @@ struct MYNTEYE_API ImuData {
   double temperature;
 
   void Reset() {
-    frame_id = 0;
+    flag = 0;
     timestamp = 0;
     std::fill(accel, accel + 3, 0);
     std::fill(gyro, gyro + 3, 0);

samples/api/camera.cc

@@ -56,8 +56,7 @@ int main(int argc, char *argv[]) {
     CHECK_NOTNULL(data.imu);
     ++imu_count;
     VLOG(2) << "Imu count: " << imu_count;
-    VLOG(2) << "  frame_id: " << data.imu->frame_id
+    VLOG(2) << ", timestamp: " << data.imu->timestamp
-            << ", timestamp: " << data.imu->timestamp
             << ", accel_x: " << data.imu->accel[0]
             << ", accel_y: " << data.imu->accel[1]
             << ", accel_z: " << data.imu->accel[2]
@@ -107,8 +106,7 @@ int main(int argc, char *argv[]) {
     auto &&motion_datas = api->GetMotionDatas();
     motion_count += motion_datas.size();
     for (auto &&data : motion_datas) {       
-      LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
+      LOG(INFO) << ", timestamp: " << data.imu->timestamp
-                << ", timestamp: " << data.imu->timestamp
                 << ", accel_x: " << data.imu->accel[0]
                 << ", accel_y: " << data.imu->accel[1]
                 << ", accel_z: " << data.imu->accel[2]

samples/device/camera.cc

@@ -71,13 +71,12 @@ int main(int argc, char *argv[]) {
       });
 
   std::size_t imu_count = 0;
-/*
+
   device->SetMotionCallback([&imu_count](const device::MotionData &data) {
     CHECK_NOTNULL(data.imu);
     ++imu_count;
     VLOG(2) << "Imu count: " << imu_count; 
-    VLOG(2) << "  frame_id: " << data.imu->frame_id
+    VLOG(2) << ", timestamp: " << data.imu->timestamp
-            << ", timestamp: " << data.imu->timestamp
             << ", accel_x: " << data.imu->accel[0]
             << ", accel_y: " << data.imu->accel[1]
             << ", accel_z: " << data.imu->accel[2]
@@ -86,7 +85,7 @@ int main(int argc, char *argv[]) {
             << ", gyro_z: " << data.imu->gyro[2]
             << ", temperature: " << data.imu->temperature;
   });
-*/
+
   // Enable this will cache the motion datas until you get them.
   // device->EnableMotionDatas();
   device->Start(Source::VIDEO_STREAMING);
@@ -99,12 +98,11 @@ int main(int argc, char *argv[]) {
 
     device::StreamData left_data = device->GetLatestStreamData(Stream::LEFT);
     device::StreamData right_data = device->GetLatestStreamData(Stream::RIGHT);
-/*
+
     auto &&motion_datas = device->GetMotionDatas();
     motion_count += motion_datas.size();
     for (auto &&data : motion_datas) {       
-      LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
+      LOG(INFO) << ", timestamp: " << data.imu->timestamp
-                << ", timestamp: " << data.imu->timestamp
                 << ", accel_x: " << data.imu->accel[0]
                 << ", accel_y: " << data.imu->accel[1]
                 << ", accel_z: " << data.imu->accel[2]
@@ -113,7 +111,7 @@ int main(int argc, char *argv[]) {
                 << ", gyro_z: " << data.imu->gyro[2]
                 << ", temperature: " << data.imu->temperature;
     }
-*/
+
     cv::Mat left_img(
         left_data.frame->height(), left_data.frame->width(), CV_8UC2,
         left_data.frame->data());
@@ -146,7 +144,7 @@ int main(int argc, char *argv[]) {
             << ", fps: " << (1000.f * right_count / elapsed_ms);
   LOG(INFO) << "Imu count: " << imu_count
             << ", hz: " << (1000.f * imu_count / elapsed_ms);
-  // LOG(INFO) << "Motion count: " << motion_count
+  LOG(INFO) << "Motion count: " << motion_count
-  //           << ", hz: " << (1000.f * motion_count / elapsed_ms);
+            << ", hz: " << (1000.f * motion_count / elapsed_ms);
   return 0;
 }

samples/tutorials/util/cv_painter.cc

@@ -118,7 +118,7 @@ cv::Rect CVPainter::DrawImuData(
   if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
     sign = -1;
 
-  Clear(ss) << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp
+  Clear(ss) << "stamp: " << data.timestamp
             << ", temp: " << fmt_temp << data.temperature;
   cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);
 

src/api/api.h

@@ -69,8 +69,7 @@ struct MYNTEYE_API MotionData {
 
   bool operator==(const MotionData &other) const {
     if (imu && other.imu) {
-      return imu->frame_id == other.imu->frame_id &&
+      return imu->timestamp == other.imu->timestamp;            
-             imu->timestamp == other.imu->timestamp;
     }
     return false;
   }

src/device/device.cc

@@ -103,14 +103,14 @@ std::shared_ptr<Device> Device::Create(
     std::string model_s = name.substr(9,5);
     VLOG(2) << "MYNE EYE Model: " << model_s;
     DeviceModel model(model_s);
-    if(model.type == 's') {
+    if(model.type == 'S') {
       switch (model.custom_code) {
         case '0':
           return std::make_shared<StandardDevice>(device);
         case 'A':
           return std::make_shared<StandardDevice>(device);
         default:
-          LOG(FATAL) << "No such custom code now"
+          LOG(FATAL) << "No such custom code now";
       }
     } else {
       LOG(FATAL) << "MYNT EYE model is not supported now";

src/internal/channels.cc

@@ -306,7 +306,7 @@ void Channels::DoImuTrack() {
     return n;
   }();
 
-  auto &&sn = res_packet.packets.back().serial_number;
+  auto &&sn = res_packet.packets.back().segments.back().serial_number;
   if (imu_sn_ == sn) {
     VLOG(2) << "New imu not ready, dropped";
     return;

src/internal/motions.cc

@@ -42,20 +42,26 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
       }
       for (auto &&seg : packet.segments) {
         auto &&imu = std::make_shared<ImuData>();
-        imu->frame_id = seg.frame_id;
+        // 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->timestamp = seg.timestamp;
-        imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
+        imu->flag = seg.flag;
-        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;
 
+        if((imu->flag) & 0x01 != 0) {
+          imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000;
+          imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000;
+          imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000;
+        } 
+        if((imu->flag) & 0x02 != 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;
+        }
+        
         std::lock_guard<std::mutex> _(mtx_datas_);
         motion_data_t data = {imu};
         motion_datas_.push_back(data);

src/internal/streams.cc

@@ -48,7 +48,7 @@ bool unpack_stereo_img_data(
   //   << ", timestamp="<< std::dec << img_packet.timestamp
   //   << ", exposure_time="<< std::dec << img_packet.exposure_time
   //   << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
-/*
+
   if (img_packet.header != 0x3B) {
     LOG(WARNING) << "Image packet header must be 0x3B, but 0x" << std::hex
                  << std::uppercase << std::setw(2) << std::setfill('0')
@@ -60,6 +60,7 @@ bool unpack_stereo_img_data(
   for (std::size_t i = 2, n = packet_n - 2; i <= n; i++) {  // content: [2,9]
     checksum = (checksum ^ packet[i]);
   }
+  /*
   if (img_packet.checksum != checksum) {
     LOG(WARNING) << "Image packet checksum should be 0x" << std::hex
                  << std::uppercase << std::setw(2) << std::setfill('0')
@@ -68,7 +69,8 @@ bool unpack_stereo_img_data(
                  << static_cast<int>(checksum) << " now";
     return false;
   }
-*/
+  */
+
   img->frame_id = img_packet.frame_id;
   img->timestamp = img_packet.timestamp;
   img->exposure_time = img_packet.exposure_time;

src/internal/types.h

@@ -148,7 +148,7 @@ struct ImagePacket {
   std::uint8_t header;
   std::uint8_t size;
   std::uint16_t frame_id;
-  std::uint32_t timestamp;
+  std::uint64_t timestamp;
   std::uint16_t exposure_time;
   std::uint8_t checksum;
 
@@ -161,10 +161,11 @@ struct ImagePacket {
     header = *data;
     size = *(data + 1);
     frame_id = (*(data + 2) << 8) | *(data + 3);
-    timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
+    timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
-                *(data + 7);
+                (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
-    exposure_time = (*(data + 8) << 8) | *(data + 9);
+                (*(data + 10) << 8) | *(data + 11);
-    checksum = *(data + 10);
+    exposure_time = (*(data + 12) << 8) | *(data + 13);
+    checksum = *(data + 14);
   }
 };
 #pragma pack(pop)
@@ -199,11 +200,11 @@ struct ImuReqPacket {
  */
 #pragma pack(push, 1)
 struct ImuSegment {
-  std::int16_t offset;
+  std::uint32_t serial_number;
-  std::uint16_t frame_id;
+  std::uint64_t timestamp;
-  std::int16_t accel[3];
+  std::uint8_t flag;
   std::int16_t temperature;
-  std::int16_t gyro[3];
+  std::int16_t aceel_or_gyro[3];
 
   ImuSegment() = default;
   explicit ImuSegment(std::uint8_t *data) {
@@ -211,15 +212,16 @@ struct ImuSegment {
   }
 
   void from_data(std::uint8_t *data) {
-    offset = (*(data) << 8) | *(data + 1);
+    serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
-    frame_id = (*(data + 2) << 8) | *(data + 3);
+                    *(data + 3);
-    accel[0] = (*(data + 4) << 8) | *(data + 5);
+    timestamp = (*(data + 4) << 56) | (*(data + 5) << 48) | (*(data + 6) << 40) |
-    accel[1] = (*(data + 6) << 8) | *(data + 7);
+                (*(data + 7) << 32) | (*(data + 8) << 24) | (*(data + 9) << 16) |
-    accel[2] = (*(data + 8) << 8) | *(data + 9);
+                (*(data + 10) << 8) | *(data + 11);
-    temperature = (*(data + 10) << 8) | *(data + 11);
+    flag = *(data + 12);
-    gyro[0] = (*(data + 12) << 8) | *(data + 13);
+    temperature = (*(data + 13) << 8) | *(data + 14);
-    gyro[1] = (*(data + 14) << 8) | *(data + 15);
+    aceel_or_gyro[0] = (*(data + 15) << 8) | *(data + 16);
-    gyro[2] = (*(data + 16) << 8) | *(data + 17);
+    aceel_or_gyro[1] = (*(data + 17) << 8) | *(data + 18);
+    aceel_or_gyro[2] = (*(data + 19) << 8) | *(data + 20);
   }
 };
 #pragma pack(pop)
@@ -230,27 +232,21 @@ struct ImuSegment {
  */
 #pragma pack(push, 1)
 struct ImuPacket {
-  std::uint32_t serial_number;
-  std::uint32_t timestamp;
   std::uint8_t count;
   std::vector<ImuSegment> segments;
 
   ImuPacket() = default;
-  explicit ImuPacket(std::uint8_t *data) {
+  
+  explicit ImuPacket(std::uint8_t seg_count,std::uint8_t *data) {
+    count = seg_count;
     from_data(data);
   }
   
   void from_data(std::uint8_t *data) {
-    serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
+        std::size_t seg_n = sizeof(ImuSegment);  // 21
-                    *(data + 3);
+        for(std::size_t i = 0; i < count; i++) {
-    timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
+          segments.push_back(ImuSegment(data + seg_n * i));
-                *(data + 7);
+        }
-    count = *(data + 8);
-
-    std::size_t seg_n = sizeof(ImuSegment);  // 18
-    for (std::size_t i = 0; i < count; i++) {
-      segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
-    }
   }
 };
 #pragma pack(pop)
@@ -277,13 +273,11 @@ struct ImuResPacket {
     state = *(data + 1);
     size = (*(data + 2) << 8) | *(data + 3);
 
-    std::size_t seg_n = sizeof(ImuSegment);  // 18
+    std::size_t seg_n = sizeof(ImuSegment);  // 21
-    for (std::size_t i = 4; i < size;) {
+    std::uint8_t seg_count = (size - 4) / seg_n;
-      ImuPacket packet(data + i);
+    ImuPacket packet(seg_count,data + 4);
-      packets.push_back(packet);
+    packets.push_back(packet);
-      i += 9 + (packet.count * seg_n);
+    //packet(2);
-    }
-
     checksum = *(data + 4 + size);
   }
 };

wrappers/python/src/mynteye_py.cc

@@ -105,8 +105,7 @@ struct MYNTEYE_API MotionData {
   ImuData imu;
 
   bool operator==(const MotionData &other) const {
-    return imu.frame_id == other.imu.frame_id &&
+    return imu.timestamp == other.imu.timestamp;           
-           imu.timestamp == other.imu.timestamp;
   }
 };
 
@@ -365,7 +364,6 @@ BOOST_PYTHON_MODULE(mynteye_py) {
   // bp::register_ptr_to_python<std::shared_ptr<ImgData>>();
 
   bp::class_<ImuData>("ImuData")
-      .def_readonly("frame_id", &ImuData::frame_id)
       .def_readonly("timestamp", &ImuData::timestamp)
       .add_property(
           "accel", +[](ImuData *o) { return array_ref<double>{o->accel}; })

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -26,7 +26,6 @@
   <arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
   <arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
 
-  <arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
   <arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
 
   <arg name="gravity" default="9.8" />
@@ -116,7 +115,6 @@
       <param name="points_frame_id" value="$(arg points_frame_id)" />
       <param name="depth_frame_id" value="$(arg depth_frame_id)" />
 
-      <param name="imu_frame_id" value="$(arg imu_frame_id)" />
       <param name="temp_frame_id" value="$(arg temp_frame_id)" />
 
       <param name="gravity" value="$(arg gravity)" />

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -326,8 +326,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       publishImu(data, imu_count_, stamp);
       publishTemp(data.imu->temperature, imu_count_, stamp);
       NODELET_DEBUG_STREAM(          
-          "Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
+          "Imu count: " << imu_count_<< ", timestamp: " << data.imu->timestamp
-                        << ", timestamp: " << data.imu->timestamp
                         << ", accel_x: " << data.imu->accel[0]
                         << ", accel_y: " << data.imu->accel[1]
                         << ", accel_z: " << data.imu->accel[2]