From 293fcae265539382b05d7795383ef29f65cb1fb3 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Tue, 26 Mar 2024 12:25:40 +0200 Subject: [PATCH 1/2] PR #12797 from noacoohen: Imu sensitivity fixes (cherry picked from commit 66abee778748f89211d44321ee2591d988011762) --- src/ds/ds-motion-common.cpp | 4 ++- src/linux/backend-hid.cpp | 3 +- src/proc/motion-transform.cpp | 53 ++++++++++++++++++++++++++--------- src/proc/motion-transform.h | 11 ++++++-- 4 files changed, 53 insertions(+), 18 deletions(-) diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 053ce786da..24193917f9 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -507,10 +507,12 @@ namespace librealsense } catch (...) {} + bool high_accuracy = _fw_version >= firmware_version( 5, 16, 0, 0 ); hid_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} }, { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} }, - [&, mm_correct_opt]() { return std::make_shared< acceleration_transform >( _mm_calib, mm_correct_opt ); + [&, mm_correct_opt, high_accuracy]() + { return std::make_shared< acceleration_transform >( _mm_calib, mm_correct_opt, high_accuracy ); }); //TODO this FW version is relevant for d400 devices. Need to change for propre d500 devices support. diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index 830430bba5..39c5b3d4ce 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -753,7 +753,8 @@ namespace librealsense bool iio_hid_sensor::has_metadata() { - if(get_output_size() == HID_DATA_ACTUAL_SIZE + HID_METADATA_SIZE) + //for FW>=5.16 HID_METADATA_SIZE is 12 + if(get_output_size() >= HID_DATA_ACTUAL_SIZE + HID_METADATA_SIZE) return true; return false; } diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index 67a575b9e5..079596d469 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -12,21 +12,40 @@ namespace librealsense { - template void copy_hid_axes( uint8_t * const dest[], const uint8_t * source, double factor, bool is_mipi) + template void copy_hid_axes( uint8_t * const dest[], const uint8_t * source, double factor, bool high_accuracy, bool is_mipi) { using namespace librealsense; auto res = float3(); - // D457 dev + if (is_mipi) { auto hid = (hid_mipi_data*)(source); - res = float3{ float(hid->x), float(hid->y), float(hid->z) } *float(factor); + + if( ! high_accuracy ) + { + //since D400 FW version 5.16 the hid report struct changed to 32 bit for each paramater. + //To support older FW versions we convert the data to int16_t before casting to float as we only get valid data at the lower 16 bits. + hid->x = static_cast< int16_t >( hid->x ); + hid->y = static_cast< int16_t >( hid->y ); + hid->z = static_cast< int16_t >( hid->z ); + } + + res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor ); } else { auto hid = (hid_data*)(source); - res = float3{ float(hid->x), float(hid->y), float(hid->z) } *float(factor); + //since D400 FW version 5.16 the hid report struct changed to 32 bit for each paramater. + //To support older FW versions we convert the data to int16_t before casting to float as we only get valid data at the lower 16 bits. + if( ! high_accuracy ) + { + hid->x = static_cast< int16_t >( hid->x ); + hid->y = static_cast< int16_t >( hid->y ); + hid->z = static_cast< int16_t >( hid->z ); + } + + res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor ); } std::memcpy( dest[0], &res, sizeof( float3 ) ); @@ -34,12 +53,12 @@ namespace librealsense // The Accelerometer input format: signed int 16bit. data units 1LSB=0.001g; // Librealsense output format: floating point 32bit. units m/s^2, - template void unpack_accel_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, bool is_mipi = false) + template void unpack_accel_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, bool high_accuracy, bool is_mipi = false) { static constexpr float gravity = 9.80665f; // Standard Gravitation Acceleration static constexpr double accelerator_transform_factor = 0.001*gravity; - copy_hid_axes(dest, source, accelerator_transform_factor, is_mipi); + copy_hid_axes< FORMAT >( dest, source, accelerator_transform_factor, high_accuracy, is_mipi ); } // The Gyro input format: signed int 16bit. data units depends on set sensitivity; @@ -49,8 +68,10 @@ namespace librealsense double gyro_scale_factor = 0.1, bool is_mipi = false ) { const double gyro_transform_factor = deg2rad( gyro_scale_factor ); - - copy_hid_axes(dest, source, gyro_transform_factor, is_mipi); + //high_accuracy=true when gyro_scale_factor=0.001 for FW version >=5.16 + //high_accuracy=false when gyro_scale_factor=0.01 for FW version <5.16 + double high_accuracy = ( gyro_scale_factor != 0.1 ); + copy_hid_axes< FORMAT >( dest, source, gyro_transform_factor, high_accuracy, is_mipi ); } motion_transform::motion_transform(rs2_format target_format, rs2_stream target_stream, @@ -194,7 +215,8 @@ namespace librealsense if (source[0] == 1) { _target_stream = RS2_STREAM_ACCEL; - unpack_accel_axes(dest, source, width, height, actual_size, true); + bool high_accuracy = ( _gyro_scale_factor != 0.1 ); + unpack_accel_axes< RS2_FORMAT_MOTION_XYZ32F >( dest, source, width, height, actual_size, high_accuracy, true ); } else if (source[0] == 2) { @@ -208,17 +230,20 @@ namespace librealsense } } - acceleration_transform::acceleration_transform(std::shared_ptr mm_calib, std::shared_ptr mm_correct_opt) - : acceleration_transform("Acceleration Transform", mm_calib, mm_correct_opt) + acceleration_transform::acceleration_transform( std::shared_ptr< mm_calib_handler > mm_calib, + std::shared_ptr< enable_motion_correction > mm_correct_opt, + bool high_accuracy ) + : acceleration_transform( "Acceleration Transform", mm_calib, mm_correct_opt, high_accuracy ) {} - acceleration_transform::acceleration_transform(const char * name, std::shared_ptr mm_calib, std::shared_ptr mm_correct_opt) - : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, mm_correct_opt) + acceleration_transform::acceleration_transform(const char * name, std::shared_ptr mm_calib, std::shared_ptr mm_correct_opt, bool high_accuracy) + : motion_transform( name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, mm_correct_opt) + , _high_accuracy( high_accuracy ) {} void acceleration_transform::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, int actual_size ) { - unpack_accel_axes(dest, source, width, height, actual_size); + unpack_accel_axes< RS2_FORMAT_MOTION_XYZ32F >( dest, source, width, height, actual_size, _high_accuracy ); } gyroscope_transform::gyroscope_transform( std::shared_ptr< mm_calib_handler > mm_calib, diff --git a/src/proc/motion-transform.h b/src/proc/motion-transform.h index 8813722bb1..8445fdd51b 100644 --- a/src/proc/motion-transform.h +++ b/src/proc/motion-transform.h @@ -59,11 +59,18 @@ namespace librealsense class acceleration_transform : public motion_transform { public: - acceleration_transform(std::shared_ptr mm_calib = nullptr, std::shared_ptr mm_correct_opt = nullptr); + acceleration_transform( std::shared_ptr< mm_calib_handler > mm_calib = nullptr, + std::shared_ptr< enable_motion_correction > mm_correct_opt = nullptr, + bool high_accuracy = false ); protected: - acceleration_transform(const char* name, std::shared_ptr mm_calib, std::shared_ptr mm_correct_opt); + acceleration_transform( const char * name, + std::shared_ptr< mm_calib_handler > mm_calib, + std::shared_ptr< enable_motion_correction > mm_correct_opt, + bool high_accuracy ); void process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size) override; + //To be refactored and change to accel_scale_factor once we implement sensitivity feature for the accel like the gyro + bool _high_accuracy = false; }; From 5074ec33ee38476e2a2ac065e1625d5649d53d77 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Tue, 26 Mar 2024 15:08:45 +0200 Subject: [PATCH 2/2] PR #12798 from noacoohen: Fix IMU back compat for D457 (cherry picked from commit 46b2b79860eaf9de224dbfa159fb31a8b3a4b9b8) --- src/platform/hid-data.h | 13 +++++++++++++ src/proc/motion-transform.cpp | 18 ++++++++---------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/platform/hid-data.h b/src/platform/hid-data.h index caa7e523b2..6c438fb5ef 100644 --- a/src/platform/hid-data.h +++ b/src/platform/hid-data.h @@ -18,6 +18,19 @@ struct hid_data struct hid_mipi_data +{ + uint8_t typeID; + uint8_t skip1; + uint64_t hwTs; + int16_t x; + int16_t y; + int16_t z; + uint64_t hwTs2; + uint64_t skip2; +}; + + +struct hid_mipi_data_32 { uint8_t typeID; uint8_t skip1; diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index 079596d469..a376a9d46b 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -20,18 +20,16 @@ namespace librealsense if (is_mipi) { - auto hid = (hid_mipi_data*)(source); - - if( ! high_accuracy ) + if( ! high_accuracy ) { - //since D400 FW version 5.16 the hid report struct changed to 32 bit for each paramater. - //To support older FW versions we convert the data to int16_t before casting to float as we only get valid data at the lower 16 bits. - hid->x = static_cast< int16_t >( hid->x ); - hid->y = static_cast< int16_t >( hid->y ); - hid->z = static_cast< int16_t >( hid->z ); + auto hid = (hid_mipi_data *)( source ); + res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor ); + } + else + { + auto hid = (hid_mipi_data_32 *)( source ); + res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor ); } - - res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor ); } else {