Skip to content

Imu sensitivity fixes #12797

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Mar 26, 2024
Merged

Imu sensitivity fixes #12797

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
885c354
fix HW timestamp error inlinux
noacoohen Mar 25, 2024
eff002b
add backward comptability support
noacoohen Mar 26, 2024
541f95e
edit comment
noacoohen Mar 26, 2024
1515c51
fix PR comments
noacoohen Mar 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion src/ds/ds-motion-common.cpp
View file
Open in desktop
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 ) ? true : false ;
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
Open in desktop
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
51 changes: 37 additions & 14 deletions src/proc/motion-transform.cpp
View file
Open in desktop
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.
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.
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,8 @@ 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);
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 +213,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 +228,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
10 changes: 8 additions & 2 deletions src/proc/motion-transform.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,17 @@ 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;
bool _high_accuracy = false;

};

Expand Down