PR #12857 from noacoohen: RSUSB support for imu sensitivity

src/ds/d400/d400-motion.cpp

@@ -134,7 +134,7 @@ namespace librealsense
             hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&hid_header::timestamp));
         }
         //for FW >=5.16 the scale factor changes to 1000.0 since FW sends 32bit
-        if (_fw_version >= firmware_version( 5, 15, 1, 224))
+        if (_fw_version >= firmware_version( 5, 16, 0, 0))
             get_raw_motion_sensor()->set_gyro_scale_factor( 10000.0 );
 
     }

src/hid/hid-device.cpp

@@ -100,7 +100,7 @@ namespace librealsense
         {
             for(auto&& p : hid_profiles)
             {
-                set_feature_report(DEVICE_POWER_D0, _sensor_to_id[p.sensor_name], p.frequency);
+                set_feature_report( DEVICE_POWER_D0, _sensor_to_id[p.sensor_name], p.frequency, p.sensitivity );
             }
             _configured_profiles = hid_profiles;
         }
@@ -161,10 +161,39 @@ namespace librealsense
                         {
                             if(!_running)
                                 return;
-                            if(r->get_actual_length() == sizeof(REALSENSE_HID_REPORT))
+
+                            if( r->get_actual_length() == _realsense_hid_report_actual_size )
                             {
+                                // for FW version < 5.16 the actual struct is 32 bytes (each IMU axis is 16 bit), so we
+                                // can not use memcpy for
+                                // the whole struct as the new struct (size 38) expect 32 bits for each.
+                                // For FW >= 5.16 we can just use memcpy as the structs size match
                                 REALSENSE_HID_REPORT report;
-                                memcpy(&report, r->get_buffer().data(), r->get_actual_length());
+                                if( _realsense_hid_report_actual_size != sizeof( REALSENSE_HID_REPORT ) )
+                                {
+
+                                    // x,y,z are all short with: x at offset 10
+                                    //                           y at offset 12
+                                    //                           z at offset 14
+                                    memcpy( &report, r->get_buffer().data(), 10 );
+                                    const int16_t * x
+                                        = reinterpret_cast< const int16_t * >( r->get_buffer().data() + 10 );
+                                    const int16_t * y
+                                        = reinterpret_cast< const int16_t * >( r->get_buffer().data() + 12 );
+                                    const int16_t * z
+                                        = reinterpret_cast< const int16_t * >( r->get_buffer().data() + 14 );
+                                    report.x = *x;
+                                    report.y = *y;
+                                    report.z = *z;
+                                    memcpy( &report.customValue1, r->get_buffer().data() + 16, 16 );
+
+                                }
+                                else
+                                {
+                                    // the rest of the data in the old struct size (after z element) starts from offset
+                                    // 16 and has 16 bytes till end
+                                    memcpy( &report, r->get_buffer().data(), r->get_actual_length() );
+                                }
                                 _queue.enqueue(std::move(report));
                             }
                             auto sts = _messenger->submit_request(r);
@@ -194,8 +223,37 @@ namespace librealsense
         void rs_hid_device::handle_interrupt()
         {
             REALSENSE_HID_REPORT report;
+
+            // for FW version < 5.16 the actual struct is 32 bytes (each IMU axis is 16 bit), so we can not use memcpy for
+            // the whole struct as the new struct (size 38) expect 32 bits for each.
+            // For FW >= 5.16 we can just use memcpy as the structs size match
+
+
 #ifdef __APPLE__
-            hid_read(_hidapi_device, reinterpret_cast<unsigned char*>(&report), sizeof(REALSENSE_HID_REPORT));
+            unsigned char tmp_buffer[100] = { 0 };
+            hid_read( _hidapi_device, tmp_buffer, _realsense_hid_report_actual_size );
+            if( _realsense_hid_report_actual_size != sizeof( REALSENSE_HID_REPORT ) )
+            {
+
+                // x,y,z are all short with: x at offset 10
+                //                           y at offset 12
+                //                           z at offset 14
+                memcpy( &report, tmp_buffer, 10 );
+                const int16_t * x = reinterpret_cast< const int16_t * >( tmp_buffer + 10 );
+                const int16_t * y = reinterpret_cast< const int16_t * >( tmp_buffer + 12 );
+                const int16_t * z = reinterpret_cast< const int16_t * >( tmp_buffer + 14 );
+                report.x = *x;
+                report.y = *y;
+                report.z = *z;
+
+                // the rest of the data in the old struct size (after z element) starts from offset 16 and has 16 bytes till end
+                memcpy( &report.customValue1, tmp_buffer + 16, 16 );
+            }
+            else
+            {
+                memcpy( &report, tmp_buffer, sizeof( REALSENSE_HID_REPORT ) );
+            }
+
             sensor_data data{};
             data.sensor = { _id_to_sensor[report.reportId] };
 
@@ -238,7 +296,7 @@ namespace librealsense
 #endif
         }
 
-        usb_status rs_hid_device::set_feature_report(unsigned char power, int report_id, int fps)
+        usb_status rs_hid_device::set_feature_report( unsigned char power, int report_id, int fps, double sensitivity)
         {
             uint32_t transferred;
 
@@ -273,6 +331,12 @@ namespace librealsense
             if(fps > 0)
                 featureReport.report = (1000 / fps);
 
+            //we want to change the sensitivity values only in gyro, for FW version >= 5.16
+            if( featureReport.reportId == REPORT_ID_GYROMETER_3D
+                && _realsense_hid_report_actual_size == sizeof( REALSENSE_HID_REPORT ) )
+                featureReport.sensitivity = sensitivity;
+
+
             res = dev->control_transfer(USB_REQUEST_CODE_SET,
                 HID_REQUEST_SET_REPORT,
                 value,
@@ -384,5 +448,13 @@ namespace librealsense
 
             return ep;
         }
+
+        void rs_hid_device::set_gyro_scale_factor(double scale_factor) 
+        {
+            _gyro_scale_factor = scale_factor;
+            // for FW >=5.16 the scale factor changes to 10000.0 since FW sends 32bit
+            if( scale_factor == 10000.0 )
+                _realsense_hid_report_actual_size = 38;
+        }
     }
 }

src/hid/hid-device.h

@@ -43,13 +43,14 @@ namespace librealsense
             virtual std::vector<uint8_t> get_custom_report_data(const std::string& custom_sensor_name,
                                                                 const std::string& report_name,
                                                                 custom_sensor_report_field report_field) override { return {}; }
-            void set_gyro_scale_factor( double scale_factor ) override{};
+            void set_gyro_scale_factor( double scale_factor ) override;
 
         private:
             void handle_interrupt();
             rs_usb_endpoint get_hid_endpoint();
             rs_usb_interface get_hid_interface();
-            usb_status set_feature_report(unsigned char power, int report_id, int fps = 0);
+            //for gyro sensitivity the default value we set in feature report is 0.1, which is mapped in FW to 30.5 millideg/sec
+            usb_status set_feature_report( unsigned char power, int report_id, int fps = 0, double sensitivity = 0.1 );
 #ifdef __APPLE__
            int hidapi_PowerDevice(unsigned char reportId);
 #endif
@@ -73,6 +74,8 @@ namespace librealsense
             std::vector<hid_profile> _configured_profiles;
             single_consumer_queue<REALSENSE_HID_REPORT> _queue;
             std::shared_ptr<active_object<>> _handle_interrupts_thread;
+            int _realsense_hid_report_actual_size = 32; // for FW version >=5.16 the struct changed to 38 bit
+            double _gyro_scale_factor = 10.0;
         };
     }
 }

src/hid/hid-types.h

@@ -8,17 +8,6 @@
 #include <string>
 #include <map>
 
-#define HID_REPORT_TYPE_INPUT       1
-#define HID_REPORT_TYPE_FEATURE     3
-
-#define DEVICE_POWER_D0             2
-#define DEVICE_POWER_D4             6
-
-#define REPORT_ID_ACCELEROMETER_3D  1
-#define REPORT_ID_GYROMETER_3D      2
-#define REPORT_ID_CUSTOM            3
-
-const size_t SIZE_OF_HID_IMU_FRAME = 32;
 
 static std::string gyro = "gyro_3d";
 static std::string accel = "accel_3d";
@@ -28,6 +17,18 @@ namespace librealsense
 {
     namespace platform
     {
+        constexpr int HID_REPORT_TYPE_INPUT = 1;
+        constexpr int HID_REPORT_TYPE_FEATURE = 3;
+        constexpr int DEVICE_POWER_D0 = 2;
+        constexpr int DEVICE_POWER_D4 = 6;
+
+    enum REPORT_ID
+    {
+        REPORT_ID_ACCELEROMETER_3D = 1,
+        REPORT_ID_GYROMETER_3D = 2,
+        REPORT_ID_CUSTOM = 3
+    };
+
     enum USB_REQUEST_CODE {
       USB_REQUEST_CODE_GET = 0xa1,
       USB_REQUEST_CODE_SET = 0x21
@@ -50,16 +51,16 @@ namespace librealsense
       unsigned char power;
       unsigned char minReport;
       unsigned short report;
-      unsigned short unknown;
+      unsigned short sensitivity;
     };
 
     struct REALSENSE_HID_REPORT {
       unsigned char reportId;
       unsigned char unknown;
       unsigned long long timeStamp;
-      short x;
-      short y;
-      short z;
+      int32_t x;
+      int32_t y;
+      int32_t z;
       unsigned int customValue1;
       unsigned int customValue2;
       unsigned short customValue3;
@@ -70,7 +71,6 @@ namespace librealsense
     };
 #pragma pack(pop)
 
-    static_assert(sizeof(REALSENSE_HID_REPORT) == SIZE_OF_HID_IMU_FRAME, "HID IMU Frame struct expected size is 32 bytes");
 
 #pragma pack(push, 1)
     struct FEATURE_REPORT
@@ -81,7 +81,7 @@ namespace librealsense
       unsigned char power;
       unsigned char minReport;
       unsigned short report;
-      unsigned short unknown;
+      unsigned short sensitivity;
     };
 #pragma pack(pop)
     }