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<rs2_format FORMAT> void copy_hid_axes( uint8_t * const dest[], const uint8_t * source, double factor, bool is_mipi)
+ template<rs2_format FORMAT> 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<rs2_format FORMAT> void unpack_accel_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, bool is_mipi = false)
+ template<rs2_format FORMAT> 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<FORMAT>(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<FORMAT>(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<RS2_FORMAT_MOTION_XYZ32F>(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_handler> mm_calib, std::shared_ptr<enable_motion_correction> 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_handler> mm_calib, std::shared_ptr<enable_motion_correction> 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_handler> mm_calib, std::shared_ptr<enable_motion_correction> 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<RS2_FORMAT_MOTION_XYZ32F>(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_handler> mm_calib = nullptr, std::shared_ptr<enable_motion_correction> 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_handler> mm_calib, std::shared_ptr<enable_motion_correction> 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;
};