Skip to content
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

Cherry Pick PR#12797 & PR#12798 #12800

Merged
merged 2 commits into from
Mar 26, 2024
Merged

Cherry Pick PR#12797 & PR#12798 #12800

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
293fcae
PR #12797 from noacoohen: Imu sensitivity fixes
Nir-Az Mar 26, 2024
5074ec3
PR #12798 from noacoohen: Fix IMU back compat for D457
Nir-Az 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 );
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
13 changes: 13 additions & 0 deletions src/platform/hid-data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
53 changes: 38 additions & 15 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,51 @@

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 )
{
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 );
}
}
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 +66,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 +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
11 changes: 9 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,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
Loading