Skip to content

Commit

Permalink
PR IntelRealSense#12797 from noacoohen: Imu sensitivity fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nir-Az
Nir-Az authored Mar 26, 2024
2 parents 9746e5e + 1515c51 commit 66abee7
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 18 deletions.
4 changes: 3 additions & 1 deletion src/ds/ds-motion-common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
3 changes: 2 additions & 1 deletion src/linux/backend-hid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
53 changes: 39 additions & 14 deletions src/proc/motion-transform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,34 +12,53 @@

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 ) );
}

// 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;
Expand All @@ -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,
Expand Down Expand Up @@ -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)
{
Expand All @@ -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,
Expand Down
11 changes: 9 additions & 2 deletions src/proc/motion-transform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

};

Expand Down

0 comments on commit 66abee7

Please sign in to comment.