diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index 760f6008f9..d6f3772a2b 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/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 ); } diff --git a/src/hid/hid-device.cpp b/src/hid/hid-device.cpp index 41d44bdc17..aaa3a18a40 100644 --- a/src/hid/hid-device.cpp +++ b/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(&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; + } } } diff --git a/src/hid/hid-device.h b/src/hid/hid-device.h index 630c645c7a..e4b478fab0 100644 --- a/src/hid/hid-device.h +++ b/src/hid/hid-device.h @@ -43,13 +43,14 @@ namespace librealsense virtual std::vector 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 _configured_profiles; single_consumer_queue _queue; std::shared_ptr> _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; }; } } diff --git a/src/hid/hid-types.h b/src/hid/hid-types.h index 797d7a89df..7f3680346f 100644 --- a/src/hid/hid-types.h +++ b/src/hid/hid-types.h @@ -8,17 +8,6 @@ #include #include -#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) }