IMU metadata attributes

src/backend.h

@@ -152,9 +152,17 @@ namespace librealsense
             uint32_t        timestamp;
             uint8_t         source_clock[6];
         };
+
+        struct hid_header
+        {
+            uint8_t         length;             // HID report total size. Limited to 255
+            uint8_t         report_type;        // Curently supported: IMU/Custom Temperature
+            uint64_t        timestamp;          // Driver-produced/FW-based timestamp. Note that currently only the lower 32bit are used
+        };
 #pragma pack(pop)
 
         constexpr uint8_t uvc_header_size = sizeof(uvc_header);
+        constexpr uint8_t hid_header_size = sizeof(hid_header);
 
         struct frame_object
         {

src/ds5/ds5-motion.cpp

@@ -319,6 +319,9 @@ namespace librealsense
 
             if (!motion_module_fw_version.empty())
                 register_info(RS2_CAMERA_INFO_FIRMWARE_VERSION, motion_module_fw_version);
+
+            // HID metadata attributes
+            hid_ep->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&platform::hid_header::timestamp));
         }
     }
 

src/l500/l500-motion.cpp

@@ -132,6 +132,9 @@ namespace librealsense
         {
             _motion_module_device_idx = add_sensor(hid_ep);
         }
+
+        // HID metadata attributes
+        hid_ep->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&platform::hid_header::timestamp));
     }
 
     std::vector<tagged_profile> l500_motion::get_profiles_tags() const

src/linux/backend-hid.cpp

@@ -1,12 +1,14 @@
 // License: Apache 2.0. See LICENSE file in root directory.
 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
 
+#include "metadata.h"
 #include "backend-hid.h"
 #include "backend.h"
 #include "types.h"
 
 #include <thread>
-
+#include <chrono>
+#include <ctime>
 #include <dirent.h>
 #include <fcntl.h>
 #include <unistd.h>
@@ -309,7 +311,7 @@ namespace librealsense
             signal_stop();
             _hid_thread->join();
             enable(false);
-            _callback = NULL;
+            _callback = nullptr;
 
             if(::close(_fd) < 0)
                 throw linux_backend_exception("hid_custom_sensor: close(_fd) failed");
@@ -504,7 +506,7 @@ namespace librealsense
             _is_capturing = true;
             _hid_thread = std::unique_ptr<std::thread>(new std::thread([this](){
                 const uint32_t channel_size = get_channel_size();
-                auto raw_data_size = channel_size*hid_buf_len;
+                size_t raw_data_size = channel_size*hid_buf_len;
 
                 std::vector<uint8_t> raw_data(raw_data_size);
                 auto metadata = has_metadata();
@@ -516,10 +518,10 @@ namespace librealsense
                     FD_SET(_stop_pipe_fd[0], &fds);
 
                     int max_fd = std::max(_stop_pipe_fd[0], _fd);
-                    auto read_size = 0;
+                    ssize_t read_size = 0;
 
                     struct timeval tv = {5, 0};
-                    auto val = select(max_fd + 1, &fds, NULL, NULL, &tv);
+                    auto val = select(max_fd + 1, &fds, nullptr, nullptr, &tv);
                     if (val < 0)
                     {
                         // TODO: write to log?
@@ -550,20 +552,41 @@ namespace librealsense
                         // TODO: code refactoring to reduce latency
                         for (auto i = 0; i < read_size / channel_size; ++i)
                         {
+                            auto now_ts = std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
                             auto p_raw_data = raw_data.data() + channel_size * i;
                             sensor_data sens_data{};
                             sens_data.sensor = hid_sensor{get_sensor_name()};
 
                             auto hid_data_size = channel_size - HID_METADATA_SIZE;
-
-                            sens_data.fo = {hid_data_size, metadata?HID_METADATA_SIZE: uint8_t(0),  p_raw_data,  metadata?p_raw_data + hid_data_size:nullptr};
+                            // Populate HID IMU data - Header
+                            metadata_hid_raw meta_data{};
+                            meta_data.header.report_type = md_hid_report_type::hid_report_imu;
+                            meta_data.header.length = hid_header_size + metadata_imu_report_size;
+                            meta_data.header.timestamp = *(reinterpret_cast<uint64_t *>(&p_raw_data[16]));
+                            // Payload:
+                            meta_data.report_type.imu_report.header.md_type_id = md_type::META_DATA_HID_IMU_REPORT_ID;
+                            meta_data.report_type.imu_report.header.md_size = metadata_imu_report_size;
+//                            meta_data.report_type.imu_report.flags = static_cast<uint8_t>( md_hid_imu_attributes::custom_timestamp_attirbute |
+//                                                                                            md_hid_imu_attributes::imu_counter_attribute |
+//                                                                                            md_hid_imu_attributes::usb_counter_attribute);
+//                            meta_data.report_type.imu_report.custom_timestamp = meta_data.header.timestamp;
+//                            meta_data.report_type.imu_report.imu_counter = p_raw_data[30];
+//                            meta_data.report_type.imu_report.usb_counter = p_raw_data[31];
+
+                            sens_data.fo = {hid_data_size, metadata? meta_data.header.length: uint8_t(0),
+                                            p_raw_data,  metadata? &meta_data : nullptr, now_ts};
                             //Linux HID provides timestamps in nanosec. Convert to usec (FW default)
                             if (metadata)
                             {
-                                auto* ts_nsec = reinterpret_cast<uint64_t*>(const_cast<void*>(sens_data.fo.metadata));
-                                *ts_nsec /=1000;
+                                //auto* ts_nsec = reinterpret_cast<uint64_t*>(const_cast<void*>(sens_data.fo.metadata));
+                                //*ts_nsec /=1000;
+                                meta_data.header.timestamp /=1000;
                             }
 
+//                            for (auto i=0ul; i<channel_size; i++)
+//                                std::cout << std::hex << int(p_raw_data[i]) << " ";
+//                            std::cout << std::dec << std::endl;
+
                             this->_callback(sens_data);
                         }
                     }
@@ -584,7 +607,7 @@ namespace librealsense
             set_power(false);
             signal_stop();
             _hid_thread->join();
-            _callback = NULL;
+            _callback = nullptr;
             _channels.clear();
 
             if(::close(_fd) < 0)

src/linux/backend-v4l2.cpp

@@ -436,7 +436,7 @@ namespace librealsense
 
                 try
                 {
-                    int vid, pid, mi;
+                    uint16_t vid, pid, mi;
                     std::string busnum, devnum, devpath;
 
                     auto dev_name = "/dev/" + name;

src/metadata-parser.h

@@ -218,6 +218,45 @@ namespace librealsense
         return parser;
     }
 
+    /**\brief A HID-Header metadata parser class*/
+    template<class St, class Attribute>
+    class md_hid_header_parser : public md_attribute_parser_base
+    {
+    public:
+        md_hid_header_parser(Attribute St::* attribute_name, attrib_modifyer mod) :
+            _md_attribute(attribute_name), _modifyer(mod) {};
+
+        rs2_metadata_type get(const librealsense::frame & frm) const override
+        {
+            if (!supports(frm))
+                throw invalid_value_exception("HID header is not available");
+
+            auto attrib = static_cast<rs2_metadata_type>((*reinterpret_cast<const St*>((const uint8_t*)frm.additional_data.metadata_blob.data())).*_md_attribute);
+            if (_modifyer) attrib = _modifyer(attrib);
+            return attrib;
+        }
+
+        bool supports(const librealsense::frame & frm) const override
+        {
+            return (frm.additional_data.metadata_size >= platform::hid_header_size);
+        }
+
+    private:
+        md_hid_header_parser() = delete;
+        md_hid_header_parser(const md_hid_header_parser&) = delete;
+
+        Attribute St::*     _md_attribute;  // Pointer to the attribute within uvc header that provides the relevant data
+        attrib_modifyer     _modifyer;      // Post-processing on received attribute
+    };
+
+    /**\brief A utility function to create HID metadata header parser*/
+    template<class St, class Attribute>
+    std::shared_ptr<md_attribute_parser_base> make_hid_header_parser(Attribute St::* attribute, attrib_modifyer mod = nullptr)
+    {
+        std::shared_ptr<md_hid_header_parser<St, Attribute>> parser(new md_hid_header_parser<St, Attribute>(attribute, mod));
+        return parser;
+    }
+
     /**\brief provide attributes generated and stored internally by the library*/
     template<class St, class Attribute>
     class md_additional_parser : public md_attribute_parser_base

src/metadata.h

@@ -42,6 +42,8 @@ namespace librealsense
         META_DATA_INTEL_L500_CAPTURE_TIMING_ID  = 0x80000010,
         META_DATA_INTEL_L500_DEPTH_CONTROL_ID   = 0x80000012,
         META_DATA_CAMERA_DEBUG_ID               = 0x800000FF,
+        META_DATA_HID_IMU_REPORT_ID             = 0x80001001,
+        META_DATA_HID_CUSTOM_TEMP_REPORT_ID     = 0x80001002,
     };
 
     static const std::map<md_type, std::string> md_type_desc =
@@ -59,6 +61,8 @@ namespace librealsense
         { md_type::META_DATA_CAMERA_DEBUG_ID,               "Camera Debug"},
         { md_type::META_DATA_INTEL_L500_CAPTURE_TIMING_ID,  "Intel Capture timing"},
         { md_type::META_DATA_INTEL_L500_DEPTH_CONTROL_ID,   "Intel Depth Control"},
+        { md_type::META_DATA_HID_IMU_REPORT_ID,             "HID IMU Report"},
+        { md_type::META_DATA_HID_CUSTOM_TEMP_REPORT_ID,     "HID Custom Temperature Report"},
     };
 
     /**\brief md_capture_timing_attributes - enumerate the bit offset to check
@@ -177,6 +181,45 @@ namespace librealsense
         right_blue_sum_attribute        = (1u << 14),
     };
 
+    /**\brief md_hid_imu_attributes - bit mask to designate the enabled attributed,
+*  md_imu_report struct */
+    enum md_hid_report_type : uint8_t
+    {
+        hid_report_unknown,
+        hid_report_imu,
+        hid_report_custom_temperature,
+        hid_report_max,
+    };
+
+    /**\brief md_hid_imu_attributes - bit mask to designate the enabled attributed,
+ *  md_imu_report struct */
+    enum class md_hid_imu_attributes : uint8_t
+    {
+        custom_timestamp_attirbute  = (1u << 0),
+        imu_counter_attribute       = (1u << 1),
+        usb_counter_attribute       = (1u << 2)
+    };
+
+    inline md_hid_imu_attributes operator |(md_hid_imu_attributes l, md_hid_imu_attributes r)
+    {
+        return static_cast<md_hid_imu_attributes>(static_cast<uint8_t>(l) | static_cast<uint8_t>(r));
+    }
+
+    inline md_hid_imu_attributes operator |=(md_hid_imu_attributes l, md_hid_imu_attributes r)
+    {
+        return l = l | r;
+    }
+
+    /**\brief md_hid_imu_attributes - bit mask to designate the enabled attributed,
+*  md_imu_report struct */
+    enum class md_hid_custom_temp_attributes : uint8_t
+    {
+        source_id_attirbute         = (1u << 0),
+        custom_timestamp_attirbute  = (1u << 1),
+        imu_counter_attribute       = (1u << 2),
+        usb_counter_attribute       = (1u << 3)
+    };
+
 #pragma pack(push, 1)
     // SR300 Metadata definitions
     struct md_sr300_rgb
@@ -590,8 +633,8 @@ namespace librealsense
      *  layout as transmitted and received by backend */
     struct metadata_raw
     {
-        platform::uvc_header   header;
-        md_modes          mode;
+        platform::uvc_header    header;
+        md_modes                mode;
     };
 
     constexpr uint8_t metadata_raw_size = sizeof(metadata_raw);
@@ -611,5 +654,48 @@ namespace librealsense
         }
     };
 
+    struct md_imu_report
+    {
+        md_header   header;
+        uint8_t     flags;              // Bit array to specify attributes that are valid (limited to 7 fields)
+        uint64_t    custom_timestamp;   // HW Timestamp
+        uint8_t     imu_counter;        // IMU internal counter
+        uint8_t     usb_counter;        // USB-layer internal counter
+    };
+
+    REGISTER_MD_TYPE(md_imu_report, md_type::META_DATA_HID_IMU_REPORT_ID)
+
+    constexpr uint8_t metadata_imu_report_size = sizeof(md_imu_report);
+
+    struct md_custom_tmp_report
+    {
+        md_header   header;
+        uint8_t     flags;              // Bit array to specify attributes that are valid (limited to 7 fields)
+        uint64_t    custom_timestamp;   // HW Timestamp
+        uint8_t     imu_counter;        // IMU internal counter
+        uint8_t     usb_counter;        // USB-layer internal counter
+        uint8_t     source_id;
+    };
+
+    REGISTER_MD_TYPE(md_custom_tmp_report, md_type::META_DATA_HID_CUSTOM_TEMP_REPORT_ID)
+
+    /**\brief md_hid_types - aggrevative structure that represents the supported HID
+ * metadata struct types to be handled */
+    union md_hid_report
+    {
+        md_imu_report           imu_report;
+        md_custom_tmp_report    temperature_report;
+    };
+
+    /**\brief metadata_hid_raw - HID metadata structure
+ *  layout populated by backend */
+    struct metadata_hid_raw
+    {
+        platform::hid_header   header;
+        md_hid_report          report_type;
+    };
+
+    constexpr uint8_t metadata_hid_raw_size = sizeof(metadata_hid_raw);
+
 #pragma pack(pop)
 }

src/mf/mf-hid.cpp

@@ -12,6 +12,7 @@
 #include "../types.h"
 #include "mf-hid.h"
 #include "win/win-helpers.h"
+#include "metadata.h"
 
 #include <PortableDeviceTypes.h>
 //#include <PortableDeviceClassExtension.h>
@@ -27,7 +28,6 @@
 #pragma comment(lib, "Sensorsapi.lib")
 #pragma comment(lib, "PortableDeviceGuids.lib")
 
-const uint8_t HID_METADATA_SIZE = 8; // bytes
 // Windows Filetime is represented in 64 - bit number of 100 - nanosecond intervals since midnight Jan 1, 1601
 // To convert to the Unix epoch, subtract 116444736000000000LL to reach Jan 1, 1970.
 constexpr uint64_t WIN_FILETIME_2_UNIX_SYSTIME = 116444736000000000LL;
@@ -121,6 +121,12 @@ namespace librealsense
                 CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_CUSTOM_VALUE2, &var));
                 auto customTimestampHigh = var.ulVal;
 
+                // Parse additional custom fields
+                CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_CUSTOM_VALUE6, &var));
+                uint8_t imu_count = var.bVal;
+                CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_CUSTOM_VALUE7, &var));
+                uint8_t usb_count = var.bVal;
+
                 /* Retrieve sensor type - Sensor types are more specific groupings than sensor categories. Sensor type IDs are GUIDs that are defined in Sensors.h */
 
                 SENSOR_TYPE_ID type{};
@@ -175,8 +181,22 @@ namespace librealsense
 
                 PropVariantClear(&var);
 
-                sensor_data d;
-                hid_sensor_data data;
+                sensor_data d{};
+                hid_sensor_data data{};
+                // Populate HID IMU data - Header
+                metadata_hid_raw meta_data{};
+                meta_data.header.report_type = md_hid_report_type::hid_report_imu;
+                meta_data.header.length = hid_header_size + metadata_imu_report_size;
+                meta_data.header.timestamp = customTimestampLow | (customTimestampHigh < 32);
+                // Payload:
+                meta_data.report_type.imu_report.header.md_type_id = md_type::META_DATA_HID_IMU_REPORT_ID;
+                meta_data.report_type.imu_report.header.md_size = metadata_imu_report_size;
+                meta_data.report_type.imu_report.flags = static_cast<uint8_t>( md_hid_imu_attributes::custom_timestamp_attirbute |
+                                                                                md_hid_imu_attributes::imu_counter_attribute |
+                                                                                md_hid_imu_attributes::usb_counter_attribute);
+                meta_data.report_type.imu_report.custom_timestamp = customTimestampLow | (customTimestampHigh < 32);
+                meta_data.report_type.imu_report.imu_counter = imu_count;
+                meta_data.report_type.imu_report.usb_counter = usb_count;
 
                 data.x = static_cast<int16_t>(rawX);
                 data.y = static_cast<int16_t>(rawY);
@@ -188,8 +208,8 @@ namespace librealsense
                 d.sensor.name = CW2A(fName);
 
                 d.fo.pixels = &data;
-                d.fo.metadata = &data.ts_low;
-                d.fo.metadata_size = HID_METADATA_SIZE;
+                d.fo.metadata = &meta_data;
+                d.fo.metadata_size = metadata_hid_raw_size;
                 d.fo.frame_size = sizeof(data);
                 d.fo.backend_time = 0;
                 if (SystemTimeToFileTime(&sys_time, &file_time))