From a764cf2d1aa8ec16d965320a277280423e54a6b7 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 28 Feb 2024 15:43:32 +0200 Subject: [PATCH 01/21] imu sensitivity feature - currently gyro only Signed-off-by: noacoohen --- include/librealsense2/h/rs_option.h | 2 + src/ds/CMakeLists.txt | 2 + src/ds/d400/d400-factory.cpp | 8 ++ src/ds/d400/d400-motion.cpp | 17 ++++ src/ds/d400/d400-motion.h | 2 + src/ds/d400/d400-options.cpp | 65 ++++++++++++- src/ds/d400/d400-options.h | 28 +++++- src/ds/ds-motion-common.cpp | 5 +- src/ds/features/imu-sensitivity-feature.cpp | 29 ++++++ src/ds/features/imu-sensitivity-feature.h | 30 ++++++ src/hid-sensor.cpp | 34 ++++++- src/hid-sensor.h | 5 +- src/linux/backend-hid.cpp | 61 +++++++++++- src/linux/backend-hid.h | 8 +- src/mf/mf-hid.cpp | 100 +++++++++++++++++--- src/platform/hid-device.h | 1 + src/to-string.cpp | 1 + 17 files changed, 375 insertions(+), 23 deletions(-) create mode 100644 src/ds/features/imu-sensitivity-feature.cpp create mode 100644 src/ds/features/imu-sensitivity-feature.h diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 7ae19cbf1f..847b673b2d 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -123,6 +123,8 @@ extern "C" { RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, /**< Select depth sensor auto exposure mode see rs2_depth_auto_exposure_mode for values */ RS2_OPTION_OHM_TEMPERATURE, /**< Temperature of the Optical Head Sensor */ RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */ + RS2_OPTION_GYRO_SENSITIVITY,/**< imu sensitivity */ + RS2_OPTION_ACCEL_SENSITIVITY,/**< imu sensitivity */ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; diff --git a/src/ds/CMakeLists.txt b/src/ds/CMakeLists.txt index f0bfecc608..24da97a261 100644 --- a/src/ds/CMakeLists.txt +++ b/src/ds/CMakeLists.txt @@ -39,4 +39,6 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/features/auto-exposure-limit-feature.cpp" "${CMAKE_CURRENT_LIST_DIR}/features/gain-limit-feature.h" "${CMAKE_CURRENT_LIST_DIR}/features/gain-limit-feature.cpp" + "${CMAKE_CURRENT_LIST_DIR}/features/imu-sensitivity-feature.h" + "${CMAKE_CURRENT_LIST_DIR}/features/imu-sensitivity-feature.cpp" ) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index f2be41122a..6c5b98e179 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -33,6 +33,8 @@ #include #include +#include +#include namespace librealsense { // PSR @@ -656,6 +658,7 @@ namespace librealsense public d400_motion, public ds_advanced_mode_base, public firmware_logger_device + { public: rs435i_device( std::shared_ptr< const d400_info > const & dev_info, bool register_device_notifications ) @@ -670,6 +673,8 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { check_and_restore_rgb_stream_extrinsic(); + if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) + register_feature( std::make_shared< imu_sensitivity_feature >(get_raw_motion_sensor(), get_motion_sensor()) ); } @@ -1004,6 +1009,9 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { + if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) + register_feature( + std::make_shared< imu_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } std::shared_ptr create_matcher(const frame_holder& frame) const override; diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index fa95ea516c..b9e1ac10cb 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -21,6 +21,7 @@ #include "proc/auto-exposure-processor.h" #include #include +#include using namespace librealsense; namespace librealsense @@ -129,6 +130,22 @@ namespace librealsense // HID metadata attributes hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&hid_header::timestamp)); } + //synthetic_sensor & check = *hid_ep; + //register_feature( std::make_shared< imu_sensitivity_feature >( check ) ); + } + + ds_motion_sensor & d400_motion::get_motion_sensor() + { + return dynamic_cast< ds_motion_sensor & >( get_sensor( _motion_module_device_idx.value() ) ); + //return std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); + + } + + std::shared_ptr d400_motion::get_raw_motion_sensor() + { + auto check = get_motion_sensor().get_raw_sensor(); + return std::dynamic_pointer_cast< hid_sensor >( check ); + // return std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); } d400_motion_uvc::d400_motion_uvc( std::shared_ptr< const d400_info > const & dev_info ) diff --git a/src/ds/d400/d400-motion.h b/src/ds/d400/d400-motion.h index 9d968584b5..dda26482c3 100644 --- a/src/ds/d400/d400-motion.h +++ b/src/ds/d400/d400-motion.h @@ -47,6 +47,8 @@ namespace librealsense std::shared_ptr create_hid_device(std::shared_ptr ctx, const std::vector& all_hid_infos, const firmware_version& camera_fw_version); + ds_motion_sensor & get_motion_sensor(); + std::shared_ptr get_raw_motion_sensor(); protected: friend class ds_motion_common; diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index cea4038c98..bc10ea4d7d 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -466,4 +466,67 @@ namespace librealsense { snapshot = std::make_shared(get_description(), 0.f); } -} + + void imu_sensitivity_option::set(float value) + { + if( auto strong = _sensor.lock() ) + { + strong->set_imu_sensitivity_resolution( RS2_STREAM_GYRO, value ); + } + } + + float imu_sensitivity_option::query() const + { + /* if( ret < get_range().min || ret > get_range().max ) + { + if( auto toggle = _exposure_limit_toggle.lock() ) + return toggle->get_cached_limit(); + }*/ + if( auto strong = _sensor.lock() ) + { + return strong->get_imu_sensitivity_resolution( RS2_STREAM_GYRO ); + } + else + return -1; + } + + + const char * librealsense::imu_sensitivity_option::get_value_description( float val ) const + { + switch( static_cast< int >( val ) ) + { + case 0: { + return "61.0"; + } + case 1: { + return "30.5"; + } + case 2: { + return "15.3"; + } + case 3: { + return "7.6"; + } + case 4: { + return "3.8"; + } + default: + throw invalid_value_exception( "value not found" ); + } + } + + const char * librealsense::imu_sensitivity_option::get_description() const + { + return "imu sensitivity resolutions"; + } + + bool librealsense::imu_sensitivity_option::is_read_only() const + { + if( auto strong = _sensor.lock() ) + return strong->is_opened(); + return false; + } + + + } + \ No newline at end of file diff --git a/src/ds/d400/d400-options.h b/src/ds/d400/d400-options.h index 3333eb70f8..a3c2a6df75 100644 --- a/src/ds/d400/d400-options.h +++ b/src/ds/d400/d400-options.h @@ -11,7 +11,7 @@ #include "../../hdr-config.h" #include - +#include namespace librealsense { @@ -188,4 +188,30 @@ namespace librealsense std::function _recording_function = [](const option&) {}; }; + + + class hid_sensor; + class imu_sensitivity_option: public option_base + { + public: + imu_sensitivity_option( const std::weak_ptr< hid_sensor > & sensor, const option_range & opt_range ) + : option_base( opt_range ) + , _sensor( sensor ){}; + virtual ~imu_sensitivity_option() = default; + virtual void set( float value ) override; + virtual float query() const override; + virtual bool is_enabled() const override { return true; } + virtual const char * get_description() const override; + const char * get_value_description( float value ) const override; + virtual void enable_recording( std::function< void( const option & ) > record_action ) override + { + _record_action = record_action; + } + virtual bool is_read_only() const override; + + private: + std::weak_ptr< hid_sensor > _sensor; + std::function< void( const option & ) > _record_action = []( const option & ) {}; + + }; } diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 9c4b7fcd43..70b584f92e 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -24,7 +24,9 @@ #include #include #include - +#include +#include +#include #include namespace librealsense @@ -492,6 +494,7 @@ namespace librealsense hid_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option); + // register pre-processing std::shared_ptr mm_correct_opt = nullptr; diff --git a/src/ds/features/imu-sensitivity-feature.cpp b/src/ds/features/imu-sensitivity-feature.cpp new file mode 100644 index 0000000000..c13d662edf --- /dev/null +++ b/src/ds/features/imu-sensitivity-feature.cpp @@ -0,0 +1,29 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#include +#include +#include +#include +#include + + +namespace librealsense { + +const feature_id imu_sensitivity_feature::ID = "Imu Sensitivity feature"; + +imu_sensitivity_feature::imu_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ) +{ + option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 0.f /*default*/ }; + //auto check = std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); + auto imu_sensitivity_control = std::make_shared< imu_sensitivity_option >( motion_sensor, enable_range ); + motion.register_option( RS2_OPTION_GYRO_SENSITIVITY, imu_sensitivity_control ); +} + + +feature_id imu_sensitivity_feature::get_id() const +{ + return ID; +} + +}//namespace librealsense diff --git a/src/ds/features/imu-sensitivity-feature.h b/src/ds/features/imu-sensitivity-feature.h new file mode 100644 index 0000000000..a4dfb9773a --- /dev/null +++ b/src/ds/features/imu-sensitivity-feature.h @@ -0,0 +1,30 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2023 Intel Corporation. All Rights Reserved. + +#pragma once + + +#include +#include +#include + +#include + +namespace librealsense { + +class synthetic_sensor; + +class imu_sensitivity_feature : public feature_interface +{ + +public: + static const feature_id ID; + + imu_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ); + + feature_id get_id() const override; + + +}; + +} // namespace librealsense diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index ab067aabad..3fbfd9a8c3 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -20,6 +20,12 @@ static const std::map< rs2_stream, uint32_t > stream_and_fourcc { RS2_STREAM_ACCEL, rs_fourcc( 'A', 'C', 'C', 'L' ) }, { RS2_STREAM_GPIO, rs_fourcc( 'G', 'P', 'I', 'O' ) } }; +static const std::map< float, float > imu_sensitivity_convert + = { { 0, 0}, + { 1.0, 0.1}, + { 2.0, 0.2}, + { 3.0, 0.3}, + { 4.0, 0.4} }; // in sensor.cpp void log_callback_end( uint32_t fps, @@ -113,7 +119,8 @@ void hid_sensor::open( const stream_profiles & requests ) _is_configured_stream[request->get_stream_type()] = true; configured_hid_profiles.push_back( platform::hid_profile{ sensor_name, - fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ) } ); + fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ), + get_imu_sensitivity_resolution_converted( request->get_stream_type() ) } ); } _hid_device->open( configured_hid_profiles ); if( Is< librealsense::global_time_interface >( _owner ) ) @@ -339,6 +346,31 @@ uint32_t hid_sensor::fps_to_sampling_frequency( rs2_stream stream, uint32_t fps else return fps; } +void hid_sensor::set_imu_sensitivity_resolution( rs2_stream stream,float value ) +{ +//is there checks needed? + _imu_sensitivity_per_rs2_stream[stream] = value; +} + +float hid_sensor::get_imu_sensitivity_resolution(rs2_stream stream) +{ + if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) + { + return _imu_sensitivity_per_rs2_stream[stream]; + } + else + return 30.5; +} + +float hid_sensor::get_imu_sensitivity_resolution_converted( rs2_stream stream ) +{ + if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) + { + return imu_sensitivity_convert.at( _imu_sensitivity_per_rs2_stream[stream] ); + } + else + return 0.1; +} iio_hid_timestamp_reader::iio_hid_timestamp_reader() { diff --git a/src/hid-sensor.h b/src/hid-sensor.h index b9207b5bbb..5b183db757 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -51,7 +51,9 @@ class hid_sensor : public raw_sensor_base void close() override; void start( rs2_frame_callback_sptr callback ) override; void stop() override; - + void set_imu_sensitivity_resolution( rs2_stream stream, float value ); + float get_imu_sensitivity_resolution( rs2_stream stream ); + float get_imu_sensitivity_resolution_converted( rs2_stream stream ); std::vector< uint8_t > get_custom_report_data( const std::string & custom_sensor_name, const std::string & report_name, platform::custom_sensor_report_field report_field ) const; @@ -69,6 +71,7 @@ class hid_sensor : public raw_sensor_base std::vector< platform::hid_sensor > _hid_sensors; std::unique_ptr< frame_timestamp_reader > _hid_iio_timestamp_reader; std::unique_ptr< frame_timestamp_reader > _custom_hid_timestamp_reader; + std::map< rs2_stream, float > _imu_sensitivity_per_rs2_stream; stream_profiles get_sensor_profiles( std::string sensor_name ) const; diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index ecbd5b951f..aa98d4d31f 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -453,7 +453,7 @@ namespace librealsense } } - iio_hid_sensor::iio_hid_sensor(const std::string& device_path, uint32_t frequency) + iio_hid_sensor::iio_hid_sensor(const std::string& device_path, uint32_t frequency, float sensitivity) : _stop_pipe_fd{}, _fd(0), _iio_device_number(0), @@ -464,7 +464,7 @@ namespace librealsense _is_capturing(false), _pm_dispatcher(16) // queue for async power management commands { - init(frequency); + init(frequency, sensitivity); } iio_hid_sensor::~iio_hid_sensor() @@ -709,6 +709,21 @@ namespace librealsense iio_device_file.close(); } + void iio_hid_sensor::set_sensitivity( float sensitivity ) + { + auto sensitivity_path = _iio_device_path + "/" + _sensitivity_name; + std::ofstream iio_device_file( sensitivity_path ); + + if( ! iio_device_file.is_open() ) + { + throw linux_backend_exception( rsutils::string::from() + << "Failed to set sensitivity " << sensitivity + << ". device path: " << sensitivity_path ); + } + iio_device_file << sensitivity; + iio_device_file.close(); + } + // Asynchronous power management void iio_hid_sensor::set_power(bool on) { @@ -763,7 +778,7 @@ namespace librealsense } // initialize the device sensor. reading its name and all of its inputs. - void iio_hid_sensor::init(uint32_t frequency) + void iio_hid_sensor::init(uint32_t frequency, float sensitivity) { std::ifstream iio_device_file(_iio_device_path + "/name"); @@ -822,10 +837,14 @@ namespace librealsense // get the specific name of sampling_frequency _sampling_frequency_name = get_sampling_frequency_name(); + // get the specific name of sensitivity + _sensitivity_name = get_sensitivity_name(); + for (auto& input : _inputs) input->enable(true); set_frequency(frequency); + set_sensitivity( sensitivity ); write_fs_attribute(_iio_device_path + "/buffer/length", hid_buf_len); } @@ -898,6 +917,36 @@ namespace librealsense return sampling_frequency_name; } + std::string iio_hid_sensor::get_sensitivity_name() const + { + std::string sensitivity_name = ""; + DIR * dir = nullptr; + struct dirent * dir_ent = nullptr; + + // start enumerate the scan elemnts dir. + dir = opendir( _iio_device_path.c_str() ); + if( dir == nullptr ) + { + throw linux_backend_exception( rsutils::string::from() + << "Failed to open scan_element " << _iio_device_path ); + } + + // verify file format. should include in_ (input) and _en (enable). + while( ( dir_ent = readdir( dir ) ) != nullptr ) + { + if( dir_ent->d_type != DT_DIR ) + { + std::string file( dir_ent->d_name ); + if( file.find( "hysteresis" ) != std::string::npos ) + { + sensitivity_name = file; + } + } + } + closedir( dir ); + return sensitivity_name; + } + // read the IIO device inputs. void iio_hid_sensor::read_device_inputs() @@ -1000,19 +1049,21 @@ namespace librealsense else { uint32_t frequency = 0; + float sensitivity = 0.1; for (auto& profile : hid_profiles) { if (profile.sensor_name == device_info.id) { frequency = profile.frequency; + sensitivity = profile.sensitivity; break; } } - if (frequency == 0) + if( frequency == 0) continue; - auto device = std::unique_ptr(new iio_hid_sensor(device_info.device_path, frequency)); + auto device = std::unique_ptr(new iio_hid_sensor(device_info.device_path, frequency, sensitivity)); _iio_hid_sensors.push_back(std::move(device)); } } diff --git a/src/linux/backend-hid.h b/src/linux/backend-hid.h index 275bb447e5..7ff4cc1033 100644 --- a/src/linux/backend-hid.h +++ b/src/linux/backend-hid.h @@ -132,7 +132,7 @@ namespace librealsense // declare device sensor with all of its inputs. class iio_hid_sensor { public: - iio_hid_sensor(const std::string& device_path, uint32_t frequency); + iio_hid_sensor(const std::string& device_path, uint32_t frequency, float sensitivity); ~iio_hid_sensor(); @@ -147,6 +147,7 @@ namespace librealsense void clear_buffer(); void set_frequency(uint32_t frequency); + void set_sensitivity( float sensitivity ); void set_power(bool on); void signal_stop(); @@ -158,7 +159,7 @@ namespace librealsense void create_channel_array(); // initialize the device sensor. reading its name and all of its inputs. - void init(uint32_t frequency); + void init(uint32_t frequency, float sensitivity); // calculate the storage size of a scan uint32_t get_channel_size() const; @@ -168,6 +169,8 @@ namespace librealsense std::string get_sampling_frequency_name() const; + std::string get_sensitivity_name() const; + // read the IIO device inputs. void read_device_inputs(); @@ -177,6 +180,7 @@ namespace librealsense std::string _iio_device_path; std::string _sensor_name; std::string _sampling_frequency_name; + std::string _sensitivity_name; std::list _inputs; std::list _channels; hid_callback _callback; diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index be211473e6..be6cc340ff 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -271,38 +271,116 @@ namespace librealsense { try { - for (auto& profile_to_open : iio_profiles) + for( auto & profile_to_open : iio_profiles ) { - for (auto& connected_sensor : _connected_sensors) + for( auto & connected_sensor : _connected_sensors ) { - if (profile_to_open.sensor_name == connected_sensor->get_sensor_name()) + if( profile_to_open.sensor_name == connected_sensor->get_sensor_name() ) { /* Set SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL sensor property to profile */ HRESULT hr = S_OK; - IPortableDeviceValues* pPropsToSet = NULL; // Input - IPortableDeviceValues* pPropsReturn = NULL; // Output + IPortableDeviceValues * pPropsToSet = NULL; // Input + IPortableDeviceValues * pPropsReturn = NULL; // Output /* Create the input object */ - CHECK_HR(CoCreateInstance(__uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pPropsToSet))); + CHECK_HR( CoCreateInstance( __uuidof( PortableDeviceValues ), + NULL, + CLSCTX_INPROC_SERVER, + IID_PPV_ARGS( &pPropsToSet ) ) ); /* Add the current report interval property */ - hr = pPropsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, profile_to_open.frequency); - if (SUCCEEDED(hr)) + hr = pPropsToSet->SetUnsignedIntegerValue( SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, + profile_to_open.frequency ); + + // bool imu_sensitivity_succeeded = true; + // if( profile_to_open.sensor_name == "HID Sensor Class Device: Gyroscope" ) + //{ + // HRESULT hr1 = S_OK; + // hr1 = pPropsToSet->SetFloatValue( SENSOR_PROPERTY_CHANGE_SENSITIVITY, + //profile_to_open.sensitivity ); + // imu_sensitivity_succeeded = SUCCEEDED( hr1 ); + //} + if( SUCCEEDED( hr ) ) { // Setting a single property - hr = connected_sensor->get_sensor()->SetProperties(pPropsToSet, &pPropsReturn); - if (SUCCEEDED(hr)) + hr = connected_sensor->get_sensor() + ->SetProperties( pPropsToSet, + &pPropsReturn ); + if( SUCCEEDED( hr ) ) { - _opened_sensors.push_back(connected_sensor); + //DWORD dwCount = 0; + //hr = pPropsReturn->GetCount( &dwCount ); + _opened_sensors.push_back( connected_sensor ); pPropsReturn->Release(); } } + pPropsToSet->Release(); + if( profile_to_open.sensor_name == "HID Sensor Class Device: Gyroscope" ) + { + // Configure sensitivity + // create an IPortableDeviceValues container for + // holding the tuples. + IPortableDeviceValues * pInSensitivityValues; + hr = ::CoCreateInstance( CLSID_PortableDeviceValues, + NULL, + CLSCTX_INPROC_SERVER, + IID_PPV_ARGS( &pInSensitivityValues ) ); + + if( SUCCEEDED( hr ) ) + { + // fill in IPortableDeviceValues container + // contents here: 0.1 G sensitivity in each of X, + // Y, and Z axes. + PROPVARIANT pv; + PropVariantInit( &pv ); + pv.vt = VT_R8; // COM type for (double) + pv.dblVal = (double)profile_to_open.sensitivity; + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, + &pv ); + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Y_DEGREES_PER_SECOND, + &pv ); + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, + &pv ); + // create an IPortableDeviceValues container for + // holding the + // tuple. + IPortableDeviceValues * pInValues = NULL; + hr = ::CoCreateInstance( CLSID_PortableDeviceValues, + NULL, + CLSCTX_INPROC_SERVER, + IID_PPV_ARGS( &pInValues ) ); + if( SUCCEEDED( hr ) ) + { + // fill it in + pInValues->SetIPortableDeviceValuesValue( SENSOR_PROPERTY_CHANGE_SENSITIVITY, + pInSensitivityValues ); + // now actually set the sensitivity + IPortableDeviceValues * pOutValues = NULL; + hr = connected_sensor->get_sensor()->SetProperties( pInValues, &pOutValues ); + if( SUCCEEDED( hr ) ) + { + // check to see if any of the setting + // requests failed + //DWORD dwCount = 0; + //hr = pOutValues->GetCount( &dwCount ); + PropVariantClear( &pv ); + } + } + pInValues->Release(); + } + } + } } } } + catch (...) { for (auto& connected_sensor : _connected_sensors) diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index 3f3f2817dc..c913a44cb8 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -20,6 +20,7 @@ struct hid_profile { std::string sensor_name; uint32_t frequency; + float sensitivity; }; diff --git a/src/to-string.cpp b/src/to-string.cpp index 446df72e72..ee5733e329 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -441,6 +441,7 @@ std::string const & get_string_( rs2_option value ) arr[RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE] = "Auto Exposure Mode"; CASE( OHM_TEMPERATURE ) CASE( SOC_PVT_TEMPERATURE ) + CASE( GYRO_SENSITIVITY ) #undef CASE return arr; }(); From a588560e01ef8df7b3e5442d8f5328b5e6218c99 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 29 Feb 2024 17:54:31 +0200 Subject: [PATCH 02/21] finished windows changes --- src/ds/CMakeLists.txt | 4 +- src/ds/d400/d400-factory.cpp | 8 +- src/ds/d400/d400-motion.cpp | 6 - src/ds/d400/d400-options.cpp | 35 ++--- src/ds/d400/d400-options.h | 6 +- src/ds/ds-motion-common.cpp | 1 - ...ature.cpp => gyro-sensitivity-feature.cpp} | 12 +- ...y-feature.h => gyro-sensitivity-feature.h} | 5 +- src/hid-sensor.cpp | 36 ++++-- src/hid-sensor.h | 6 +- src/linux/backend-hid.cpp | 4 +- src/mf/mf-hid.cpp | 120 +++++++----------- src/platform/hid-device.h | 2 +- 13 files changed, 111 insertions(+), 134 deletions(-) rename src/ds/features/{imu-sensitivity-feature.cpp => gyro-sensitivity-feature.cpp} (50%) rename src/ds/features/{imu-sensitivity-feature.h => gyro-sensitivity-feature.h} (68%) diff --git a/src/ds/CMakeLists.txt b/src/ds/CMakeLists.txt index 24da97a261..f40a4aeaa0 100644 --- a/src/ds/CMakeLists.txt +++ b/src/ds/CMakeLists.txt @@ -39,6 +39,6 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/features/auto-exposure-limit-feature.cpp" "${CMAKE_CURRENT_LIST_DIR}/features/gain-limit-feature.h" "${CMAKE_CURRENT_LIST_DIR}/features/gain-limit-feature.cpp" - "${CMAKE_CURRENT_LIST_DIR}/features/imu-sensitivity-feature.h" - "${CMAKE_CURRENT_LIST_DIR}/features/imu-sensitivity-feature.cpp" + "${CMAKE_CURRENT_LIST_DIR}/features/gyro-sensitivity-feature.h" + "${CMAKE_CURRENT_LIST_DIR}/features/gyro-sensitivity-feature.cpp" ) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 6c5b98e179..4e5e268b59 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -33,7 +33,7 @@ #include #include -#include +#include #include namespace librealsense { @@ -674,7 +674,8 @@ namespace librealsense { check_and_restore_rgb_stream_extrinsic(); if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) - register_feature( std::make_shared< imu_sensitivity_feature >(get_raw_motion_sensor(), get_motion_sensor()) ); + register_feature( + std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } @@ -1009,9 +1010,10 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { + //change to 5.16 if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) register_feature( - std::make_shared< imu_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); + std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } std::shared_ptr create_matcher(const frame_holder& frame) const override; diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index b9e1ac10cb..ae266829d5 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -21,7 +21,6 @@ #include "proc/auto-exposure-processor.h" #include #include -#include using namespace librealsense; namespace librealsense @@ -130,22 +129,17 @@ namespace librealsense // HID metadata attributes hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&hid_header::timestamp)); } - //synthetic_sensor & check = *hid_ep; - //register_feature( std::make_shared< imu_sensitivity_feature >( check ) ); } ds_motion_sensor & d400_motion::get_motion_sensor() { return dynamic_cast< ds_motion_sensor & >( get_sensor( _motion_module_device_idx.value() ) ); - //return std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); - } std::shared_ptr d400_motion::get_raw_motion_sensor() { auto check = get_motion_sensor().get_raw_sensor(); return std::dynamic_pointer_cast< hid_sensor >( check ); - // return std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); } d400_motion_uvc::d400_motion_uvc( std::shared_ptr< const d400_info > const & dev_info ) diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index bc10ea4d7d..cc213104d3 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -467,31 +467,36 @@ namespace librealsense snapshot = std::make_shared(get_description(), 0.f); } - void imu_sensitivity_option::set(float value) + void librealsense::gyro_sensitivity_option::set( float value ) { + auto strong = _sensor.lock(); + if( ! strong ) + throw invalid_value_exception( "Hid sensor is not alive for setting" ); + + if( strong->is_streaming() ) + throw invalid_value_exception( "setting this option during streaming is not allowed!" ); + + if(!is_valid(value)) + throw invalid_value_exception( "set(gyro_sensitivity) failed! Invalid Gyro sensitivity resolution request " + + std::to_string( value ) ); + if( auto strong = _sensor.lock() ) { - strong->set_imu_sensitivity_resolution( RS2_STREAM_GYRO, value ); + strong->set_imu_sensitivity( RS2_STREAM_GYRO, value ); } } - float imu_sensitivity_option::query() const + float librealsense::gyro_sensitivity_option::query() const { - /* if( ret < get_range().min || ret > get_range().max ) - { - if( auto toggle = _exposure_limit_toggle.lock() ) - return toggle->get_cached_limit(); - }*/ if( auto strong = _sensor.lock() ) - { - return strong->get_imu_sensitivity_resolution( RS2_STREAM_GYRO ); - } + return strong->get_imu_sensitivity( RS2_STREAM_GYRO ); else return -1; + } - const char * librealsense::imu_sensitivity_option::get_value_description( float val ) const + const char * librealsense::gyro_sensitivity_option::get_value_description( float val ) const { switch( static_cast< int >( val ) ) { @@ -515,12 +520,12 @@ namespace librealsense } } - const char * librealsense::imu_sensitivity_option::get_description() const + const char * librealsense::gyro_sensitivity_option::get_description() const { - return "imu sensitivity resolutions"; + return "gyro sensitivity resolutions"; } - bool librealsense::imu_sensitivity_option::is_read_only() const + bool librealsense::gyro_sensitivity_option::is_read_only() const { if( auto strong = _sensor.lock() ) return strong->is_opened(); diff --git a/src/ds/d400/d400-options.h b/src/ds/d400/d400-options.h index a3c2a6df75..951b8f7434 100644 --- a/src/ds/d400/d400-options.h +++ b/src/ds/d400/d400-options.h @@ -191,13 +191,13 @@ namespace librealsense class hid_sensor; - class imu_sensitivity_option: public option_base + class gyro_sensitivity_option: public option_base { public: - imu_sensitivity_option( const std::weak_ptr< hid_sensor > & sensor, const option_range & opt_range ) + gyro_sensitivity_option( const std::weak_ptr< hid_sensor > & sensor, const option_range & opt_range ) : option_base( opt_range ) , _sensor( sensor ){}; - virtual ~imu_sensitivity_option() = default; + virtual ~gyro_sensitivity_option() = default; virtual void set( float value ) override; virtual float query() const override; virtual bool is_enabled() const override { return true; } diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 70b584f92e..3f5dabc2d1 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include diff --git a/src/ds/features/imu-sensitivity-feature.cpp b/src/ds/features/gyro-sensitivity-feature.cpp similarity index 50% rename from src/ds/features/imu-sensitivity-feature.cpp rename to src/ds/features/gyro-sensitivity-feature.cpp index c13d662edf..c0cf36284c 100644 --- a/src/ds/features/imu-sensitivity-feature.cpp +++ b/src/ds/features/gyro-sensitivity-feature.cpp @@ -1,7 +1,7 @@ // License: Apache 2.0. See LICENSE file in root directory. // Copyright(c) 2023 Intel Corporation. All Rights Reserved. -#include +#include #include #include #include @@ -10,18 +10,18 @@ namespace librealsense { -const feature_id imu_sensitivity_feature::ID = "Imu Sensitivity feature"; +const feature_id gyro_sensitivity_feature::ID = "Gyro Sensitivity feature"; -imu_sensitivity_feature::imu_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ) +gyro_sensitivity_feature::gyro_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, + ds_motion_sensor & motion ) { option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 0.f /*default*/ }; - //auto check = std::dynamic_pointer_cast< hid_sensor >( motion_sensor.get_raw_sensor() ); - auto imu_sensitivity_control = std::make_shared< imu_sensitivity_option >( motion_sensor, enable_range ); + auto imu_sensitivity_control = std::make_shared< gyro_sensitivity_option >( motion_sensor, enable_range ); motion.register_option( RS2_OPTION_GYRO_SENSITIVITY, imu_sensitivity_control ); } -feature_id imu_sensitivity_feature::get_id() const +feature_id gyro_sensitivity_feature::get_id() const { return ID; } diff --git a/src/ds/features/imu-sensitivity-feature.h b/src/ds/features/gyro-sensitivity-feature.h similarity index 68% rename from src/ds/features/imu-sensitivity-feature.h rename to src/ds/features/gyro-sensitivity-feature.h index a4dfb9773a..6a3a03c7d3 100644 --- a/src/ds/features/imu-sensitivity-feature.h +++ b/src/ds/features/gyro-sensitivity-feature.h @@ -12,15 +12,14 @@ namespace librealsense { -class synthetic_sensor; -class imu_sensitivity_feature : public feature_interface +class gyro_sensitivity_feature : public feature_interface { public: static const feature_id ID; - imu_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ); + gyro_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ); feature_id get_id() const override; diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 3fbfd9a8c3..a7c0d48f8c 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -20,12 +20,17 @@ static const std::map< rs2_stream, uint32_t > stream_and_fourcc { RS2_STREAM_ACCEL, rs_fourcc( 'A', 'C', 'C', 'L' ) }, { RS2_STREAM_GPIO, rs_fourcc( 'G', 'P', 'I', 'O' ) } }; -static const std::map< float, float > imu_sensitivity_convert - = { { 0, 0}, - { 1.0, 0.1}, - { 2.0, 0.2}, - { 3.0, 0.3}, - { 4.0, 0.4} }; +/*For gyro sensitivity - FW gets 0 for 61 milligegree/s/LSB resolution + 0.1 for 30.5 milligegree/s/LSB + 0.2 for 15.3 milligegree/s/LSB + 0.3 for 7.6 milligegree/s/LSB + 0.4 for milligegree/s/LSB */ +static const std::map< float, double > gyro_sensitivity_convert + = { { 0.0f, 0}, + { 1.0f, 0.1}, + { 2.0f, 0.2}, + { 3.0f, 0.3}, + { 4.0f, 0.4} }; // in sensor.cpp void log_callback_end( uint32_t fps, @@ -120,7 +125,7 @@ void hid_sensor::open( const stream_profiles & requests ) configured_hid_profiles.push_back( platform::hid_profile{ sensor_name, fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ), - get_imu_sensitivity_resolution_converted( request->get_stream_type() ) } ); + get_imu_sensitivity_converted( request->get_stream_type() ) } ); } _hid_device->open( configured_hid_profiles ); if( Is< librealsense::global_time_interface >( _owner ) ) @@ -346,30 +351,33 @@ uint32_t hid_sensor::fps_to_sampling_frequency( rs2_stream stream, uint32_t fps else return fps; } -void hid_sensor::set_imu_sensitivity_resolution( rs2_stream stream,float value ) +void hid_sensor::set_imu_sensitivity( rs2_stream stream, float value ) { -//is there checks needed? _imu_sensitivity_per_rs2_stream[stream] = value; } -float hid_sensor::get_imu_sensitivity_resolution(rs2_stream stream) +float hid_sensor::get_imu_sensitivity(rs2_stream stream) { if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) { return _imu_sensitivity_per_rs2_stream[stream]; } else - return 30.5; + // gyro sensitivity default value is +-1000 therefore returning 30.5 resolution + // accel sensitivity default value is +-4g therefore returning 1.95 resolution + return stream == RS2_STREAM_GYRO ? 30.5f : 1.95f; } -float hid_sensor::get_imu_sensitivity_resolution_converted( rs2_stream stream ) +double hid_sensor::get_imu_sensitivity_converted( rs2_stream stream ) { if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) { - return imu_sensitivity_convert.at( _imu_sensitivity_per_rs2_stream[stream] ); + return gyro_sensitivity_convert.at( _imu_sensitivity_per_rs2_stream[stream] ); } else - return 0.1; + //gyro sensitivity default value is +-1000 therefore sending 0.1 + //accel sensitivity default value is +-4g therefore sensong 0.001 + return stream == RS2_STREAM_GYRO ? 0.1f : 0.001f; } iio_hid_timestamp_reader::iio_hid_timestamp_reader() diff --git a/src/hid-sensor.h b/src/hid-sensor.h index 5b183db757..0ef73fa82f 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -51,9 +51,9 @@ class hid_sensor : public raw_sensor_base void close() override; void start( rs2_frame_callback_sptr callback ) override; void stop() override; - void set_imu_sensitivity_resolution( rs2_stream stream, float value ); - float get_imu_sensitivity_resolution( rs2_stream stream ); - float get_imu_sensitivity_resolution_converted( rs2_stream stream ); + void set_imu_sensitivity( rs2_stream stream, float value ); + float get_imu_sensitivity( rs2_stream stream ); + double get_imu_sensitivity_converted( rs2_stream stream ); std::vector< uint8_t > get_custom_report_data( const std::string & custom_sensor_name, const std::string & report_name, platform::custom_sensor_report_field report_field ) const; diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index aa98d4d31f..802c2817a8 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -1049,7 +1049,7 @@ namespace librealsense else { uint32_t frequency = 0; - float sensitivity = 0.1; + float sensitivity = 0; for (auto& profile : hid_profiles) { if (profile.sensor_name == device_info.id) @@ -1060,7 +1060,7 @@ namespace librealsense } } - if( frequency == 0) + if(frequency == 0) continue; auto device = std::unique_ptr(new iio_hid_sensor(device_info.device_path, frequency, sensitivity)); diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index be6cc340ff..ab2ee7318d 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -283,97 +283,67 @@ namespace librealsense IPortableDeviceValues * pPropsReturn = NULL; // Output /* Create the input object */ - CHECK_HR( CoCreateInstance( __uuidof( PortableDeviceValues ), - NULL, - CLSCTX_INPROC_SERVER, - IID_PPV_ARGS( &pPropsToSet ) ) ); + CHECK_HR( CoCreateInstance( __uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pPropsToSet))); /* Add the current report interval property */ - hr = pPropsToSet->SetUnsignedIntegerValue( SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, - profile_to_open.frequency ); - - // bool imu_sensitivity_succeeded = true; - // if( profile_to_open.sensor_name == "HID Sensor Class Device: Gyroscope" ) - //{ - // HRESULT hr1 = S_OK; - // hr1 = pPropsToSet->SetFloatValue( SENSOR_PROPERTY_CHANGE_SENSITIVITY, - //profile_to_open.sensitivity ); - // imu_sensitivity_succeeded = SUCCEEDED( hr1 ); - //} - if( SUCCEEDED( hr ) ) + hr = pPropsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, profile_to_open.frequency); + if(SUCCEEDED(hr)) { // Setting a single property - hr = connected_sensor->get_sensor() - ->SetProperties( pPropsToSet, - &pPropsReturn ); - if( SUCCEEDED( hr ) ) + hr = connected_sensor->get_sensor()->SetProperties(pPropsToSet, &pPropsReturn); + if(SUCCEEDED(hr)) { - //DWORD dwCount = 0; - //hr = pPropsReturn->GetCount( &dwCount ); - _opened_sensors.push_back( connected_sensor ); + _opened_sensors.push_back(connected_sensor); pPropsReturn->Release(); } } - pPropsToSet->Release(); + + //currently implemented only for Gyro sensitivity if( profile_to_open.sensor_name == "HID Sensor Class Device: Gyroscope" ) { - // Configure sensitivity - // create an IPortableDeviceValues container for - // holding the tuples. + // creating IPortableDeviceValues container for tuples IPortableDeviceValues * pInSensitivityValues; - hr = ::CoCreateInstance( CLSID_PortableDeviceValues, + CHECK_HR( CoCreateInstance( CLSID_PortableDeviceValues, NULL, CLSCTX_INPROC_SERVER, - IID_PPV_ARGS( &pInSensitivityValues ) ); - + IID_PPV_ARGS( &pInSensitivityValues ) )); + + PROPVARIANT pv; + PropVariantInit( &pv ); + // COM type for double + pv.vt = VT_R8; + //pv.vt = VT_R4; + pv.dblVal = (double)profile_to_open.sensitivity; + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, + &pv ); + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Y_DEGREES_PER_SECOND, + &pv ); + pInSensitivityValues->SetValue( + SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, + &pv ); + // creating IPortableDeviceValues container holding tuple + IPortableDeviceValues * pInValues = NULL; //Input + CHECK_HR(CoCreateInstance( CLSID_PortableDeviceValues, + NULL, + CLSCTX_INPROC_SERVER, + IID_PPV_ARGS( &pInValues ) )); + + pInValues->SetIPortableDeviceValuesValue( SENSOR_PROPERTY_CHANGE_SENSITIVITY, + pInSensitivityValues ); + + IPortableDeviceValues * pOutValues = NULL; //Output + // set sensitivity + hr = connected_sensor->get_sensor()->SetProperties( pInValues, &pOutValues ); if( SUCCEEDED( hr ) ) - { - // fill in IPortableDeviceValues container - // contents here: 0.1 G sensitivity in each of X, - // Y, and Z axes. - PROPVARIANT pv; - PropVariantInit( &pv ); - pv.vt = VT_R8; // COM type for (double) - pv.dblVal = (double)profile_to_open.sensitivity; - pInSensitivityValues->SetValue( - SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, - &pv ); - pInSensitivityValues->SetValue( - SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Y_DEGREES_PER_SECOND, - &pv ); - pInSensitivityValues->SetValue( - SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, - &pv ); - // create an IPortableDeviceValues container for - // holding the - // tuple. - IPortableDeviceValues * pInValues = NULL; - hr = ::CoCreateInstance( CLSID_PortableDeviceValues, - NULL, - CLSCTX_INPROC_SERVER, - IID_PPV_ARGS( &pInValues ) ); - if( SUCCEEDED( hr ) ) - { - // fill it in - pInValues->SetIPortableDeviceValuesValue( SENSOR_PROPERTY_CHANGE_SENSITIVITY, - pInSensitivityValues ); - // now actually set the sensitivity - IPortableDeviceValues * pOutValues = NULL; - hr = connected_sensor->get_sensor()->SetProperties( pInValues, &pOutValues ); - if( SUCCEEDED( hr ) ) - { - // check to see if any of the setting - // requests failed - //DWORD dwCount = 0; - //hr = pOutValues->GetCount( &dwCount ); - PropVariantClear( &pv ); - } - } - pInValues->Release(); - } + PropVariantClear( &pv ); + + pInValues->Release(); + } } diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index c913a44cb8..0d78cffa53 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -20,7 +20,7 @@ struct hid_profile { std::string sensor_name; uint32_t frequency; - float sensitivity; + double sensitivity; }; From cd444eb3b0b31d8312dc9ecb6e2ea3d494612535 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 29 Feb 2024 18:26:02 +0200 Subject: [PATCH 03/21] registering feature from FW version 5.16 --- src/ds/d400/d400-factory.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 4e5e268b59..2a4bdcc384 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -673,7 +673,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { check_and_restore_rgb_stream_extrinsic(); - if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) + if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } @@ -1010,8 +1010,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { - //change to 5.16 - if( _fw_version >= firmware_version( 5, 12, 10, 11 ) ) + if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } From f1ceb35871f3d3a8de438b14d8a5d54f43a4d4ed Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 29 Feb 2024 19:22:22 +0200 Subject: [PATCH 04/21] enabling earlier FW version for validation checks --- src/ds/d400/d400-factory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 2a4bdcc384..c9107d3e73 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -673,7 +673,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { check_and_restore_rgb_stream_extrinsic(); - if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) + if( _fw_version >= firmware_version( 5, 15, 1, 4 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } @@ -1010,7 +1010,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { - if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) + if( _fw_version >= firmware_version( 5, 15, 1, 4 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } From fb138d4b605fc8ad11e6658a300c31be6862ed10 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 29 Feb 2024 21:46:49 +0200 Subject: [PATCH 05/21] removing spaces --- src/ds/d400/d400-factory.cpp | 1 - src/ds/ds-motion-common.cpp | 3 --- src/hid-sensor.cpp | 4 ++-- src/linux/backend-hid.cpp | 2 +- src/mf/mf-hid.cpp | 17 ++++++++--------- 5 files changed, 11 insertions(+), 16 deletions(-) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index c9107d3e73..76a548745a 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -658,7 +658,6 @@ namespace librealsense public d400_motion, public ds_advanced_mode_base, public firmware_logger_device - { public: rs435i_device( std::shared_ptr< const d400_info > const & dev_info, bool register_device_notifications ) diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 3f5dabc2d1..e9f710ee7e 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -24,8 +24,6 @@ #include #include #include -#include -#include #include namespace librealsense @@ -493,7 +491,6 @@ namespace librealsense hid_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option); - // register pre-processing std::shared_ptr mm_correct_opt = nullptr; diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index a7c0d48f8c..6908abce40 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -124,8 +124,8 @@ void hid_sensor::open( const stream_profiles & requests ) _is_configured_stream[request->get_stream_type()] = true; configured_hid_profiles.push_back( platform::hid_profile{ sensor_name, - fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ), - get_imu_sensitivity_converted( request->get_stream_type() ) } ); + fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ), + get_imu_sensitivity_converted( request->get_stream_type() ) } ); } _hid_device->open( configured_hid_profiles ); if( Is< librealsense::global_time_interface >( _owner ) ) diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index 802c2817a8..4368cff7b5 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -1060,7 +1060,7 @@ namespace librealsense } } - if(frequency == 0) + if (frequency == 0) continue; auto device = std::unique_ptr(new iio_hid_sensor(device_info.device_path, frequency, sensitivity)); diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index ab2ee7318d..97fc1f880a 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -271,27 +271,27 @@ namespace librealsense { try { - for( auto & profile_to_open : iio_profiles ) + for (auto& profile_to_open : iio_profiles) { - for( auto & connected_sensor : _connected_sensors ) + for (auto& connected_sensor : _connected_sensors) { - if( profile_to_open.sensor_name == connected_sensor->get_sensor_name() ) + if (profile_to_open.sensor_name == connected_sensor->get_sensor_name()) { /* Set SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL sensor property to profile */ HRESULT hr = S_OK; - IPortableDeviceValues * pPropsToSet = NULL; // Input - IPortableDeviceValues * pPropsReturn = NULL; // Output + IPortableDeviceValues* pPropsToSet = NULL; // Input + IPortableDeviceValues* pPropsReturn = NULL; // Output /* Create the input object */ - CHECK_HR( CoCreateInstance( __uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pPropsToSet))); + CHECK_HR(CoCreateInstance(__uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pPropsToSet))); /* Add the current report interval property */ hr = pPropsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, profile_to_open.frequency); - if(SUCCEEDED(hr)) + if (SUCCEEDED(hr)) { // Setting a single property hr = connected_sensor->get_sensor()->SetProperties(pPropsToSet, &pPropsReturn); - if(SUCCEEDED(hr)) + if (SUCCEEDED(hr)) { _opened_sensors.push_back(connected_sensor); pPropsReturn->Release(); @@ -350,7 +350,6 @@ namespace librealsense } } } - catch (...) { for (auto& connected_sensor : _connected_sensors) From 7367a8c49a4f868fca6a8587019b240dfb65b281 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 29 Feb 2024 21:50:30 +0200 Subject: [PATCH 06/21] removing spaces --- src/ds/ds-motion-common.cpp | 1 + src/hid-sensor.cpp | 2 +- src/mf/mf-hid.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index e9f710ee7e..9c4b7fcd43 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -24,6 +24,7 @@ #include #include #include + #include namespace librealsense diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 6908abce40..0287d16c37 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -124,7 +124,7 @@ void hid_sensor::open( const stream_profiles & requests ) _is_configured_stream[request->get_stream_type()] = true; configured_hid_profiles.push_back( platform::hid_profile{ sensor_name, - fps_to_sampling_frequency( request->get_stream_type(), request->get_framerate() ), + fps_to_sampling_frequency(request->get_stream_type(), request->get_framerate()), get_imu_sensitivity_converted( request->get_stream_type() ) } ); } _hid_device->open( configured_hid_profiles ); diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index 97fc1f880a..bf1db5d101 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -279,7 +279,7 @@ namespace librealsense { /* Set SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL sensor property to profile */ HRESULT hr = S_OK; - IPortableDeviceValues* pPropsToSet = NULL; // Input + IPortableDeviceValues* pPropsToSet = NULL; // Input IPortableDeviceValues* pPropsReturn = NULL; // Output /* Create the input object */ From a12299c89f4592b4d833c7c88ff7b7cc45108c8b Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 4 Mar 2024 13:26:25 +0200 Subject: [PATCH 07/21] PR comments fixing --- include/librealsense2/h/rs_option.h | 3 +-- src/ds/d400/d400-factory.cpp | 2 +- src/ds/d400/d400-motion.cpp | 4 ++-- src/ds/d400/d400-options.cpp | 12 ++++------ src/ds/features/gyro-sensitivity-feature.cpp | 2 +- src/ds/features/gyro-sensitivity-feature.h | 2 +- src/hid-sensor.cpp | 25 +++++++++----------- src/hid-sensor.h | 4 ++-- src/linux/backend-hid.cpp | 2 ++ src/mf/mf-hid.cpp | 4 +--- 10 files changed, 27 insertions(+), 33 deletions(-) diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 847b673b2d..29ddd6db3d 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -123,8 +123,7 @@ extern "C" { RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, /**< Select depth sensor auto exposure mode see rs2_depth_auto_exposure_mode for values */ RS2_OPTION_OHM_TEMPERATURE, /**< Temperature of the Optical Head Sensor */ RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */ - RS2_OPTION_GYRO_SENSITIVITY,/**< imu sensitivity */ - RS2_OPTION_ACCEL_SENSITIVITY,/**< imu sensitivity */ + RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level */ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 76a548745a..c79a284860 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -34,7 +34,7 @@ #include #include #include -#include + namespace librealsense { // PSR diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index ae266829d5..a48fd8b62e 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -138,8 +138,8 @@ namespace librealsense std::shared_ptr d400_motion::get_raw_motion_sensor() { - auto check = get_motion_sensor().get_raw_sensor(); - return std::dynamic_pointer_cast< hid_sensor >( check ); + auto raw_sensor = get_motion_sensor().get_raw_sensor(); + return std::dynamic_pointer_cast< hid_sensor >( raw_sensor ); } d400_motion_uvc::d400_motion_uvc( std::shared_ptr< const d400_info > const & dev_info ) diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index cc213104d3..34f856c56b 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -480,19 +480,17 @@ namespace librealsense throw invalid_value_exception( "set(gyro_sensitivity) failed! Invalid Gyro sensitivity resolution request " + std::to_string( value ) ); - if( auto strong = _sensor.lock() ) - { - strong->set_imu_sensitivity( RS2_STREAM_GYRO, value ); - } + + strong->set_imu_sensitivity( RS2_STREAM_GYRO, value ); + } float librealsense::gyro_sensitivity_option::query() const { if( auto strong = _sensor.lock() ) - return strong->get_imu_sensitivity( RS2_STREAM_GYRO ); + return strong->get_imu_sensitivity_raw_values( RS2_STREAM_GYRO ); else - return -1; - + throw invalid_value_exception( "gyro_sensitivity_option::query result is empty!" ); } diff --git a/src/ds/features/gyro-sensitivity-feature.cpp b/src/ds/features/gyro-sensitivity-feature.cpp index c0cf36284c..36bcadd946 100644 --- a/src/ds/features/gyro-sensitivity-feature.cpp +++ b/src/ds/features/gyro-sensitivity-feature.cpp @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #include #include diff --git a/src/ds/features/gyro-sensitivity-feature.h b/src/ds/features/gyro-sensitivity-feature.h index 6a3a03c7d3..2b35d848ba 100644 --- a/src/ds/features/gyro-sensitivity-feature.h +++ b/src/ds/features/gyro-sensitivity-feature.h @@ -1,5 +1,5 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #pragma once diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 0287d16c37..ea95c7a22a 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -24,13 +24,10 @@ static const std::map< rs2_stream, uint32_t > stream_and_fourcc 0.1 for 30.5 milligegree/s/LSB 0.2 for 15.3 milligegree/s/LSB 0.3 for 7.6 milligegree/s/LSB - 0.4 for milligegree/s/LSB */ + 0.4 for 3.8 milligegree/s/LSB + Currently it is intended for D400 devices, when this feature will be added to D500 the convert needs to be checked*/ static const std::map< float, double > gyro_sensitivity_convert - = { { 0.0f, 0}, - { 1.0f, 0.1}, - { 2.0f, 0.2}, - { 3.0f, 0.3}, - { 4.0f, 0.4} }; + = { { 0.0f, 0 }, { 1.0f, 0.1 }, { 2.0f, 0.2 }, { 3.0f, 0.3 }, { 4.0f, 0.4 } }; // in sensor.cpp void log_callback_end( uint32_t fps, @@ -125,7 +122,7 @@ void hid_sensor::open( const stream_profiles & requests ) configured_hid_profiles.push_back( platform::hid_profile{ sensor_name, fps_to_sampling_frequency(request->get_stream_type(), request->get_framerate()), - get_imu_sensitivity_converted( request->get_stream_type() ) } ); + get_imu_sensitivity_values( request->get_stream_type() ) } ); } _hid_device->open( configured_hid_profiles ); if( Is< librealsense::global_time_interface >( _owner ) ) @@ -356,19 +353,19 @@ void hid_sensor::set_imu_sensitivity( rs2_stream stream, float value ) _imu_sensitivity_per_rs2_stream[stream] = value; } -float hid_sensor::get_imu_sensitivity(rs2_stream stream) +float hid_sensor::get_imu_sensitivity_raw_values( rs2_stream stream ) { if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) - { return _imu_sensitivity_per_rs2_stream[stream]; - } else // gyro sensitivity default value is +-1000 therefore returning 30.5 resolution - // accel sensitivity default value is +-4g therefore returning 1.95 resolution - return stream == RS2_STREAM_GYRO ? 30.5f : 1.95f; + // accel sensitivity default value is +-4g therefore returning 1.95 resolution + return stream == RS2_STREAM_GYRO ? 30.5f : 1.95f; } -double hid_sensor::get_imu_sensitivity_converted( rs2_stream stream ) +/*For gyro sensitivity - FW expects 0/0.1/0.2/0.3/0.4 we convert the values from the enum 0/1/2/3/4 +the user choose to the values FW expects using gyro_sensitivity_convert*/ +double hid_sensor::get_imu_sensitivity_values( rs2_stream stream ) { if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) { @@ -376,7 +373,7 @@ double hid_sensor::get_imu_sensitivity_converted( rs2_stream stream ) } else //gyro sensitivity default value is +-1000 therefore sending 0.1 - //accel sensitivity default value is +-4g therefore sensong 0.001 + //accel sensitivity default value is +-4g therefore sending 0.001 return stream == RS2_STREAM_GYRO ? 0.1f : 0.001f; } diff --git a/src/hid-sensor.h b/src/hid-sensor.h index 0ef73fa82f..1683638bcc 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -52,8 +52,8 @@ class hid_sensor : public raw_sensor_base void start( rs2_frame_callback_sptr callback ) override; void stop() override; void set_imu_sensitivity( rs2_stream stream, float value ); - float get_imu_sensitivity( rs2_stream stream ); - double get_imu_sensitivity_converted( rs2_stream stream ); + float get_imu_sensitivity_raw_values( rs2_stream stream ); + double get_imu_sensitivity_values( rs2_stream stream ); std::vector< uint8_t > get_custom_report_data( const std::string & custom_sensor_name, const std::string & report_name, platform::custom_sensor_report_field report_field ) const; diff --git a/src/linux/backend-hid.cpp b/src/linux/backend-hid.cpp index 4368cff7b5..830430bba5 100644 --- a/src/linux/backend-hid.cpp +++ b/src/linux/backend-hid.cpp @@ -917,6 +917,8 @@ namespace librealsense return sampling_frequency_name; } + /*The sensitivity value we need to transfer to FW is stored in a file named with "hysteresis" in its title. + Therefore, we are searching for a file that includes "hysteresis" in its name.*/ std::string iio_hid_sensor::get_sensitivity_name() const { std::string sensitivity_name = ""; diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index bf1db5d101..11ba96b128 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -314,7 +314,6 @@ namespace librealsense PropVariantInit( &pv ); // COM type for double pv.vt = VT_R8; - //pv.vt = VT_R4; pv.dblVal = (double)profile_to_open.sensitivity; pInSensitivityValues->SetValue( SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, @@ -325,8 +324,7 @@ namespace librealsense pInSensitivityValues->SetValue( SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &pv ); - // creating IPortableDeviceValues container holding tuple + // creating IPortableDeviceValues container holding tuple IPortableDeviceValues * pInValues = NULL; //Input CHECK_HR(CoCreateInstance( CLSID_PortableDeviceValues, NULL, From 5f336533add14a20c5bdf36cc63034342b6ed98c Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 4 Mar 2024 13:37:49 +0200 Subject: [PATCH 08/21] Add explanation for the map use in hi-sensor.h --- src/hid-sensor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/hid-sensor.h b/src/hid-sensor.h index 1683638bcc..4f0a0f44fc 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -71,6 +71,7 @@ class hid_sensor : public raw_sensor_base std::vector< platform::hid_sensor > _hid_sensors; std::unique_ptr< frame_timestamp_reader > _hid_iio_timestamp_reader; std::unique_ptr< frame_timestamp_reader > _custom_hid_timestamp_reader; + //this map is intended to keep the values for gyro and accel sensitivity if the user changed them, available from FW >=5.16 std::map< rs2_stream, float > _imu_sensitivity_per_rs2_stream; stream_profiles get_sensor_profiles( std::string sensor_name ) const; From c9c38b7aee0c2afc4a89119b77a7b37ed6394038 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 4 Mar 2024 17:39:30 +0200 Subject: [PATCH 09/21] fix PR comments --- src/ds/d400/d400-motion.cpp | 2 +- src/ds/d400/d400-options.cpp | 2 +- src/ds/d400/d400-options.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index a48fd8b62e..9f7dd68820 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -21,7 +21,7 @@ #include "proc/auto-exposure-processor.h" #include #include - +#include using namespace librealsense; namespace librealsense { diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index 34f856c56b..f1f0a7c064 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -6,7 +6,7 @@ #include "d400-options.h" #include - +#include namespace librealsense { diff --git a/src/ds/d400/d400-options.h b/src/ds/d400/d400-options.h index 951b8f7434..46b6647f8d 100644 --- a/src/ds/d400/d400-options.h +++ b/src/ds/d400/d400-options.h @@ -11,7 +11,7 @@ #include "../../hdr-config.h" #include -#include + namespace librealsense { From 848b8e5c3373576ebb7355ebede502bb07e25495 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 5 Mar 2024 11:25:45 +0200 Subject: [PATCH 10/21] add rs2_gyro_sensitivity enum --- include/librealsense2/h/rs_option.h | 14 +++++++++++++- src/core/enum-helpers.h | 1 + src/ds/d400/d400-options.cpp | 10 +++++----- src/realsense.def | 1 + src/to-string.cpp | 18 ++++++++++++++++++ 5 files changed, 38 insertions(+), 6 deletions(-) diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 29ddd6db3d..e6cea81488 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -123,7 +123,7 @@ extern "C" { RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, /**< Select depth sensor auto exposure mode see rs2_depth_auto_exposure_mode for values */ RS2_OPTION_OHM_TEMPERATURE, /**< Temperature of the Optical Head Sensor */ RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */ - RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level */ + RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level, see rs2_gyro_sensitivity for values */ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; @@ -273,6 +273,18 @@ extern "C" { } rs2_depth_auto_exposure_mode; const char* rs2_depth_auto_exposure_mode_to_string( rs2_depth_auto_exposure_mode mode ); + /** \brief values for RS2_OPTION_GYRO_SENSITIVITY option. */ + typedef enum rs2_gyro_sensitivity + { + RS2_OPTION_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC = 0, + RS2_OPTION_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC = 1, + RS2_OPTION_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC = 2, + RS2_OPTION_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC = 3, + RS2_OPTION_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC = 4, + RS2_OPTION_GYRO_SENSITIVITY_COUNT + } rs2_gyro_sensitivity; + const char * rs2_gyro_sensitivity_to_string( rs2_gyro_sensitivity mode ); + /** * check if an option is read-only * \param[in] options the options container diff --git a/src/core/enum-helpers.h b/src/core/enum-helpers.h index 02123efcf3..c58cfac2bc 100644 --- a/src/core/enum-helpers.h +++ b/src/core/enum-helpers.h @@ -78,6 +78,7 @@ RS2_ENUM_HELPERS_CUSTOMIZED( rs2_digital_gain, RS2_DIGITAL_GAIN_HIGH, RS2_DIGITA RS2_ENUM_HELPERS( rs2_host_perf_mode, HOST_PERF ) RS2_ENUM_HELPERS( rs2_emitter_frequency_mode, EMITTER_FREQUENCY ) RS2_ENUM_HELPERS( rs2_depth_auto_exposure_mode, DEPTH_AUTO_EXPOSURE ) +RS2_ENUM_HELPERS( rs2_gyro_sensitivity, OPTION_GYRO_SENSITIVITY ) } // namespace librealsense diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index f1f0a7c064..981f55afe1 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -498,19 +498,19 @@ namespace librealsense { switch( static_cast< int >( val ) ) { - case 0: { + case RS2_OPTION_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC: { return "61.0"; } - case 1: { + case RS2_OPTION_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC: { return "30.5"; } - case 2: { + case RS2_OPTION_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC: { return "15.3"; } - case 3: { + case RS2_OPTION_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC: { return "7.6"; } - case 4: { + case RS2_OPTION_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC: { return "3.8"; } default: diff --git a/src/realsense.def b/src/realsense.def index 9ba6890cdb..bb6a51a7ed 100644 --- a/src/realsense.def +++ b/src/realsense.def @@ -263,6 +263,7 @@ EXPORTS rs2_serialize_json rs2_emitter_frequency_mode_to_string rs2_depth_auto_exposure_mode_to_string + rs2_gyro_sensitivity_to_string rs2_create_record_device rs2_create_record_device_ex diff --git a/src/to-string.cpp b/src/to-string.cpp index ee5733e329..2aacd2fe03 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -235,6 +235,23 @@ const char * get_string( rs2_depth_auto_exposure_mode mode ) #undef CASE } +const char * get_string( rs2_gyro_sensitivity value ) +{ +#define CASE( X ) STRCASE( OPTION_GYRO_SENSITIVITY, X ) + switch( value ) + { + CASE( 61_0_MILLI_DEG_SEC ) + CASE( 30_5_MILLI_DEG_SEC ) + CASE( 15_3_MILLI_DEG_SEC ) + CASE( 7_6_MILLI_DEG_SEC ) + CASE( 3_8_MILLI_DEG_SEC ) + default: + assert( ! is_valid( value ) ); + return UNKNOWN_VALUE; + } +#undef CASE +} + const char * get_string( rs2_extension value ) { #define CASE( X ) STRCASE( EXTENSION, X ) @@ -785,3 +802,4 @@ const char * rs2_calibration_status_to_string( rs2_calibration_status status ) { const char * rs2_host_perf_mode_to_string( rs2_host_perf_mode mode ) { return librealsense::get_string( mode ); } const char * rs2_emitter_frequency_mode_to_string( rs2_emitter_frequency_mode mode ) { return librealsense::get_string( mode ); } const char * rs2_depth_auto_exposure_mode_to_string( rs2_depth_auto_exposure_mode mode ) { return librealsense::get_string( mode ); } +const char * rs2_gyro_sensitivity_to_string( rs2_gyro_sensitivity mode ){return librealsense::get_string( mode );} From d4612fcfb85653d7b1a78a55b887ec5119eb0c20 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 6 Mar 2024 09:50:30 +0200 Subject: [PATCH 11/21] remove option from enum name and fix typo --- include/librealsense2/h/rs_option.h | 12 ++++++------ src/core/enum-helpers.h | 2 +- src/ds/d400/d400-options.cpp | 10 +++++----- src/hid-sensor.cpp | 10 +++++----- src/to-string.cpp | 2 +- 5 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index e6cea81488..c14ab11de9 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -276,12 +276,12 @@ extern "C" { /** \brief values for RS2_OPTION_GYRO_SENSITIVITY option. */ typedef enum rs2_gyro_sensitivity { - RS2_OPTION_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC = 0, - RS2_OPTION_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC = 1, - RS2_OPTION_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC = 2, - RS2_OPTION_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC = 3, - RS2_OPTION_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC = 4, - RS2_OPTION_GYRO_SENSITIVITY_COUNT + RS2_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC = 0, + RS2_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC = 1, + RS2_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC = 2, + RS2_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC = 3, + RS2_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC = 4, + RS2_GYRO_SENSITIVITY_COUNT } rs2_gyro_sensitivity; const char * rs2_gyro_sensitivity_to_string( rs2_gyro_sensitivity mode ); diff --git a/src/core/enum-helpers.h b/src/core/enum-helpers.h index c58cfac2bc..0b856acbd6 100644 --- a/src/core/enum-helpers.h +++ b/src/core/enum-helpers.h @@ -78,7 +78,7 @@ RS2_ENUM_HELPERS_CUSTOMIZED( rs2_digital_gain, RS2_DIGITAL_GAIN_HIGH, RS2_DIGITA RS2_ENUM_HELPERS( rs2_host_perf_mode, HOST_PERF ) RS2_ENUM_HELPERS( rs2_emitter_frequency_mode, EMITTER_FREQUENCY ) RS2_ENUM_HELPERS( rs2_depth_auto_exposure_mode, DEPTH_AUTO_EXPOSURE ) -RS2_ENUM_HELPERS( rs2_gyro_sensitivity, OPTION_GYRO_SENSITIVITY ) +RS2_ENUM_HELPERS( rs2_gyro_sensitivity, GYRO_SENSITIVITY ) } // namespace librealsense diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index 981f55afe1..b4570a13c0 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -498,19 +498,19 @@ namespace librealsense { switch( static_cast< int >( val ) ) { - case RS2_OPTION_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC: { + case RS2_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC: { return "61.0"; } - case RS2_OPTION_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC: { + case RS2_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC: { return "30.5"; } - case RS2_OPTION_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC: { + case RS2_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC: { return "15.3"; } - case RS2_OPTION_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC: { + case RS2_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC: { return "7.6"; } - case RS2_OPTION_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC: { + case RS2_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC: { return "3.8"; } default: diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index ea95c7a22a..e96e82bea9 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -20,11 +20,11 @@ static const std::map< rs2_stream, uint32_t > stream_and_fourcc { RS2_STREAM_ACCEL, rs_fourcc( 'A', 'C', 'C', 'L' ) }, { RS2_STREAM_GPIO, rs_fourcc( 'G', 'P', 'I', 'O' ) } }; -/*For gyro sensitivity - FW gets 0 for 61 milligegree/s/LSB resolution - 0.1 for 30.5 milligegree/s/LSB - 0.2 for 15.3 milligegree/s/LSB - 0.3 for 7.6 milligegree/s/LSB - 0.4 for 3.8 milligegree/s/LSB +/*For gyro sensitivity - FW gets 0 for 61 millidegree/s/LSB resolution + 0.1 for 30.5 millidegree/s/LSB + 0.2 for 15.3 millidegree/s/LSB + 0.3 for 7.6 millidegree/s/LSB + 0.4 for 3.8 millidegree/s/LSB Currently it is intended for D400 devices, when this feature will be added to D500 the convert needs to be checked*/ static const std::map< float, double > gyro_sensitivity_convert = { { 0.0f, 0 }, { 1.0f, 0.1 }, { 2.0f, 0.2 }, { 3.0f, 0.3 }, { 4.0f, 0.4 } }; diff --git a/src/to-string.cpp b/src/to-string.cpp index 2aacd2fe03..a99ee8abb3 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -237,7 +237,7 @@ const char * get_string( rs2_depth_auto_exposure_mode mode ) const char * get_string( rs2_gyro_sensitivity value ) { -#define CASE( X ) STRCASE( OPTION_GYRO_SENSITIVITY, X ) +#define CASE( X ) STRCASE( GYRO_SENSITIVITY, X ) switch( value ) { CASE( 61_0_MILLI_DEG_SEC ) From be9d8d10d1c1d41769042098b0bbd21544183e54 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 6 Mar 2024 11:00:59 +0200 Subject: [PATCH 12/21] align the unpack_gyro_axes function comment --- src/proc/motion-transform.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index c7e56127a7..671e6759ba 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -48,8 +48,8 @@ namespace librealsense void unpack_gyro_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, bool high_sensitivity = false, bool is_mipi = false ) { - // Default sensitivity is +-2000 deg/sec at 16.384 LSB/Deg/Sec (LSB is 0.1220703125 deg/sec, historically rounded to 0.1). - // High sensitivity is +-125 deg/sec at 262.144 LSB/Deg/Sec (LSB is 0.003814697265625 deg/sec). + // Default sensitivity in HKR is +-2000 deg/sec at 16.384 LSB/Deg/Sec. + // High sensitivity is +-125 deg/sec at 262.144 LSB/Deg/Sec (LSB is 0.003814697265625 deg/sec), in legacy the calculations happen in FW and we need scalefactor 0.1. const double gyro_transform_factor = deg2rad( high_sensitivity ? 0.003814697265625 : 0.1 ); copy_hid_axes(dest, source, gyro_transform_factor, is_mipi); From debca975e2c250e5281b51a770d3dcc608e00cf6 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Sun, 10 Mar 2024 12:38:13 +0200 Subject: [PATCH 13/21] changing scale factor for Validation checks --- src/mf/mf-hid.cpp | 2 +- src/proc/motion-transform.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index 11ba96b128..40ecc28624 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -167,7 +167,7 @@ namespace librealsense CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &var)); rawZ = var.dblVal; - static constexpr double gyro_transform_factor = 10.0; + static constexpr double gyro_transform_factor = 10000.0; rawX *= gyro_transform_factor; rawY *= gyro_transform_factor; diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index 671e6759ba..d0bd0e6cd3 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -50,7 +50,7 @@ namespace librealsense { // Default sensitivity in HKR is +-2000 deg/sec at 16.384 LSB/Deg/Sec. // High sensitivity is +-125 deg/sec at 262.144 LSB/Deg/Sec (LSB is 0.003814697265625 deg/sec), in legacy the calculations happen in FW and we need scalefactor 0.1. - const double gyro_transform_factor = deg2rad( high_sensitivity ? 0.003814697265625 : 0.1 ); + const double gyro_transform_factor = deg2rad( high_sensitivity ? 0.003814697265625 : 0.0001 ); copy_hid_axes(dest, source, gyro_transform_factor, is_mipi); } From ee69912cc4bac8d5685c77de5ea1f18b2b15d4f6 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 11 Mar 2024 15:50:00 +0200 Subject: [PATCH 14/21] changing hid structs to 32 bit, add default values to gyro option and chnge default value in viewer --- src/ds/d400/d400-options.cpp | 8 ++------ src/ds/d400/d400-options.h | 7 ++++++- src/ds/features/gyro-sensitivity-feature.cpp | 2 +- src/hid-sensor.cpp | 12 +----------- src/hid-sensor.h | 1 - src/mf/mf-hid.cpp | 6 +++--- src/platform/hid-data.h | 12 ++++++------ src/platform/hid-device.h | 12 ++++++------ 8 files changed, 25 insertions(+), 35 deletions(-) diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index b4570a13c0..0686f3f356 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -480,17 +480,13 @@ namespace librealsense throw invalid_value_exception( "set(gyro_sensitivity) failed! Invalid Gyro sensitivity resolution request " + std::to_string( value ) ); - + _value = value; strong->set_imu_sensitivity( RS2_STREAM_GYRO, value ); - } float librealsense::gyro_sensitivity_option::query() const { - if( auto strong = _sensor.lock() ) - return strong->get_imu_sensitivity_raw_values( RS2_STREAM_GYRO ); - else - throw invalid_value_exception( "gyro_sensitivity_option::query result is empty!" ); + return _value; } diff --git a/src/ds/d400/d400-options.h b/src/ds/d400/d400-options.h index 46b6647f8d..046cb8cde0 100644 --- a/src/ds/d400/d400-options.h +++ b/src/ds/d400/d400-options.h @@ -196,7 +196,11 @@ namespace librealsense public: gyro_sensitivity_option( const std::weak_ptr< hid_sensor > & sensor, const option_range & opt_range ) : option_base( opt_range ) - , _sensor( sensor ){}; + , _value( opt_range.def ) + , _sensor( sensor ) + { + set( _value ); + } virtual ~gyro_sensitivity_option() = default; virtual void set( float value ) override; virtual float query() const override; @@ -210,6 +214,7 @@ namespace librealsense virtual bool is_read_only() const override; private: + float _value; std::weak_ptr< hid_sensor > _sensor; std::function< void( const option & ) > _record_action = []( const option & ) {}; diff --git a/src/ds/features/gyro-sensitivity-feature.cpp b/src/ds/features/gyro-sensitivity-feature.cpp index 36bcadd946..b09c717260 100644 --- a/src/ds/features/gyro-sensitivity-feature.cpp +++ b/src/ds/features/gyro-sensitivity-feature.cpp @@ -15,7 +15,7 @@ const feature_id gyro_sensitivity_feature::ID = "Gyro Sensitivity feature"; gyro_sensitivity_feature::gyro_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion ) { - option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 0.f /*default*/ }; + option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 1.f /*default*/ }; auto imu_sensitivity_control = std::make_shared< gyro_sensitivity_option >( motion_sensor, enable_range ); motion.register_option( RS2_OPTION_GYRO_SENSITIVITY, imu_sensitivity_control ); } diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index e96e82bea9..a4b50ed2a7 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -353,18 +353,8 @@ void hid_sensor::set_imu_sensitivity( rs2_stream stream, float value ) _imu_sensitivity_per_rs2_stream[stream] = value; } -float hid_sensor::get_imu_sensitivity_raw_values( rs2_stream stream ) -{ - if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) - return _imu_sensitivity_per_rs2_stream[stream]; - else - // gyro sensitivity default value is +-1000 therefore returning 30.5 resolution - // accel sensitivity default value is +-4g therefore returning 1.95 resolution - return stream == RS2_STREAM_GYRO ? 30.5f : 1.95f; -} - /*For gyro sensitivity - FW expects 0/0.1/0.2/0.3/0.4 we convert the values from the enum 0/1/2/3/4 -the user choose to the values FW expects using gyro_sensitivity_convert*/ +the user chooses to the values FW expects using gyro_sensitivity_convert*/ double hid_sensor::get_imu_sensitivity_values( rs2_stream stream ) { if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() ) diff --git a/src/hid-sensor.h b/src/hid-sensor.h index 4f0a0f44fc..50eb0cccf1 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -52,7 +52,6 @@ class hid_sensor : public raw_sensor_base void start( rs2_frame_callback_sptr callback ) override; void stop() override; void set_imu_sensitivity( rs2_stream stream, float value ); - float get_imu_sensitivity_raw_values( rs2_stream stream ); double get_imu_sensitivity_values( rs2_stream stream ); std::vector< uint8_t > get_custom_report_data( const std::string & custom_sensor_name, const std::string & report_name, diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index 40ecc28624..f7c8ef0db8 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -198,9 +198,9 @@ namespace librealsense meta_data.report_type.imu_report.imu_counter = imu_count; meta_data.report_type.imu_report.usb_counter = usb_count; - data.x = static_cast(rawX); - data.y = static_cast(rawY); - data.z = static_cast(rawZ); + data.x = static_cast(rawX); + data.y = static_cast(rawY); + data.z = static_cast(rawZ); data.ts_low = customTimestampLow; data.ts_high = customTimestampHigh; diff --git a/src/platform/hid-data.h b/src/platform/hid-data.h index eab62b492d..b4cbd71404 100644 --- a/src/platform/hid-data.h +++ b/src/platform/hid-data.h @@ -11,12 +11,12 @@ namespace librealsense { #pragma pack( push, 1 ) struct hid_data { - short x; - uint8_t reserved1[2]; - short y; - uint8_t reserved2[2]; - short z; - uint8_t reserved3[2]; + int32_t x; + //uint16_t reserved1[2]; + int32_t y; + //uint16_t reserved2[2]; + int32_t z; + //uint16_t reserved3[2]; }; diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index 0d78cffa53..a53c4e172c 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -44,12 +44,12 @@ struct sensor_data #pragma pack( push, 1 ) struct hid_sensor_data { - short x; - char reserved1[2]; - short y; - char reserved2[2]; - short z; - char reserved3[2]; + int32_t x; + //char reserved1[2]; + int32_t y; + //char reserved2[2]; + int32_t z; + //char reserved3[2]; uint32_t ts_low; uint32_t ts_high; }; From 3a4e6d49694266152e0e379de3c9fd508b843fe7 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 12 Mar 2024 16:28:18 +0200 Subject: [PATCH 15/21] add changing gyro_scale_factor --- src/ds/d400/d400-factory.cpp | 4 +-- src/ds/ds-motion-common.cpp | 5 ++-- src/ds/features/gyro-sensitivity-feature.cpp | 1 + src/hid-sensor.cpp | 5 ++++ src/hid-sensor.h | 1 + src/mf/mf-hid.cpp | 18 +++++++----- src/mf/mf-hid.h | 2 ++ src/platform/hid-device.h | 1 + src/proc/motion-transform.cpp | 29 ++++++++++++-------- src/proc/motion-transform.h | 12 ++++---- 10 files changed, 49 insertions(+), 29 deletions(-) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index c79a284860..b12eefc108 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -672,7 +672,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { check_and_restore_rgb_stream_extrinsic(); - if( _fw_version >= firmware_version( 5, 15, 1, 4 ) ) + if( _fw_version >= firmware_version( 5, 15, 1, 224 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } @@ -1009,7 +1009,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { - if( _fw_version >= firmware_version( 5, 15, 1, 4 ) ) + if( _fw_version >= firmware_version( 5, 15, 1, 224 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 9c4b7fcd43..385bc5359b 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -514,11 +514,12 @@ namespace librealsense }); bool high_sensitivity = _owner->is_gyro_high_sensitivity(); + double gyro_scale_factor = high_sensitivity ? 0.003814697265625 : ( _fw_version >= firmware_version( 5, 15, 1, 224 ) ? 0.0001: 0.1 ); hid_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, - [&, mm_correct_opt, high_sensitivity]() { - return std::make_shared< gyroscope_transform >( _mm_calib, mm_correct_opt, high_sensitivity ); + [&, mm_correct_opt, gyro_scale_factor]() { + return std::make_shared< gyroscope_transform >( _mm_calib, mm_correct_opt, gyro_scale_factor ); }); return hid_ep; diff --git a/src/ds/features/gyro-sensitivity-feature.cpp b/src/ds/features/gyro-sensitivity-feature.cpp index b09c717260..ab48a144d1 100644 --- a/src/ds/features/gyro-sensitivity-feature.cpp +++ b/src/ds/features/gyro-sensitivity-feature.cpp @@ -18,6 +18,7 @@ gyro_sensitivity_feature::gyro_sensitivity_feature( std::shared_ptr< hid_sensor option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 1.f /*default*/ }; auto imu_sensitivity_control = std::make_shared< gyro_sensitivity_option >( motion_sensor, enable_range ); motion.register_option( RS2_OPTION_GYRO_SENSITIVITY, imu_sensitivity_control ); + motion_sensor->set_gyro_scale_factor( 10000.0 ); } diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index a4b50ed2a7..274a4f48cd 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -353,6 +353,11 @@ void hid_sensor::set_imu_sensitivity( rs2_stream stream, float value ) _imu_sensitivity_per_rs2_stream[stream] = value; } +void hid_sensor::set_gyro_scale_factor(double scale_factor) +{ + _hid_device->set_gyro_scale_factor( scale_factor ); +} + /*For gyro sensitivity - FW expects 0/0.1/0.2/0.3/0.4 we convert the values from the enum 0/1/2/3/4 the user chooses to the values FW expects using gyro_sensitivity_convert*/ double hid_sensor::get_imu_sensitivity_values( rs2_stream stream ) diff --git a/src/hid-sensor.h b/src/hid-sensor.h index 50eb0cccf1..cc4ca6908f 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -53,6 +53,7 @@ class hid_sensor : public raw_sensor_base void stop() override; void set_imu_sensitivity( rs2_stream stream, float value ); double get_imu_sensitivity_values( rs2_stream stream ); + void set_gyro_scale_factor(double scale_factor); std::vector< uint8_t > get_custom_report_data( const std::string & custom_sensor_name, const std::string & report_name, platform::custom_sensor_report_field report_field ) const; diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index f7c8ef0db8..919cff1ee3 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -41,7 +41,7 @@ namespace librealsense public: virtual ~sensor_events() = default; - explicit sensor_events(hid_callback callback) : m_cRef(0), _callback(callback) {} + explicit sensor_events(hid_callback callback, double gyro_scale_factor = 10.0) : m_cRef(0), _callback(callback), _gyro_scale_factor(gyro_scale_factor) {} STDMETHODIMP QueryInterface(REFIID iid, void** ppv) { @@ -167,11 +167,9 @@ namespace librealsense CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &var)); rawZ = var.dblVal; - static constexpr double gyro_transform_factor = 10000.0; - - rawX *= gyro_transform_factor; - rawY *= gyro_transform_factor; - rawZ *= gyro_transform_factor; + rawX *= _gyro_scale_factor; + rawY *= _gyro_scale_factor; + rawZ *= _gyro_scale_factor; } else { @@ -265,6 +263,7 @@ namespace librealsense private: long m_cRef; hid_callback _callback; + double _gyro_scale_factor = 10.0; }; void wmf_hid_device::open(const std::vector&iio_profiles) @@ -372,7 +371,7 @@ namespace librealsense void wmf_hid_device::start_capture(hid_callback callback) { // Hack, start default profile - _cb = new sensor_events(callback); + _cb = new sensor_events(callback, _gyro_scale_factor); ISensorEvents* sensorEvents = nullptr; CHECK_HR(_cb->QueryInterface(IID_PPV_ARGS(&sensorEvents))); @@ -406,6 +405,11 @@ namespace librealsense return std::vector(); } + void wmf_hid_device::set_gyro_scale_factor(double scale_factor) + { + _gyro_scale_factor = scale_factor; + } + void wmf_hid_device::foreach_hid_device(std::function)> action) { /* Enumerate all HID devices and run action function on each device */ diff --git a/src/mf/mf-hid.h b/src/mf/mf-hid.h index 28e73d60d7..9f84d4a6f6 100644 --- a/src/mf/mf-hid.h +++ b/src/mf/mf-hid.h @@ -67,6 +67,7 @@ namespace librealsense void start_capture(hid_callback callback) override; std::vector get_sensors() override; // Get opened sensors std::vector get_custom_report_data(const std::string& custom_sensor_name, const std::string& report_name, custom_sensor_report_field report_field) override; + void set_gyro_scale_factor( double scale_factor ) override; private: // Don't move the position of wmf_backend member. This object must be destroyed only after COM objects. @@ -78,6 +79,7 @@ namespace librealsense CComPtr _cb; std::vector _hid_profiles; + double _gyro_scale_factor = 10.0; }; } } diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index a53c4e172c..99507a0fb1 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -86,6 +86,7 @@ class hid_device const std::string & report_name, custom_sensor_report_field report_field ) = 0; + virtual void set_gyro_scale_factor( double scale_factor ) = 0; }; diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index d0bd0e6cd3..0248bf8864 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -46,11 +46,11 @@ namespace librealsense // Librealsense output format: floating point 32bit. units rad/sec, template< rs2_format FORMAT > void unpack_gyro_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, - bool high_sensitivity = false, bool is_mipi = false ) + double gyro_scale_factor = 0.1, bool is_mipi = false ) { // Default sensitivity in HKR is +-2000 deg/sec at 16.384 LSB/Deg/Sec. // High sensitivity is +-125 deg/sec at 262.144 LSB/Deg/Sec (LSB is 0.003814697265625 deg/sec), in legacy the calculations happen in FW and we need scalefactor 0.1. - const double gyro_transform_factor = deg2rad( high_sensitivity ? 0.003814697265625 : 0.0001 ); + const double gyro_transform_factor = deg2rad( gyro_scale_factor ); copy_hid_axes(dest, source, gyro_transform_factor, is_mipi); } @@ -125,16 +125,16 @@ namespace librealsense motion_to_accel_gyro::motion_to_accel_gyro( std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity ) - : motion_to_accel_gyro( "Accel_Gyro Transform", mm_calib, mm_correct_opt, high_sensitivity ) + double gyro_scale_factor ) + : motion_to_accel_gyro( "Accel_Gyro Transform", mm_calib, mm_correct_opt, gyro_scale_factor ) {} motion_to_accel_gyro::motion_to_accel_gyro( const char * name, std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity ) + double gyro_scale_factor ) : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ANY, mm_calib, mm_correct_opt) - , _high_sensitivity( high_sensitivity ) + , _gyro_scale_factor(gyro_scale_factor) { configure_processing_callback(); } @@ -202,7 +202,7 @@ namespace librealsense { _target_stream = RS2_STREAM_GYRO; unpack_gyro_axes( dest, source, width, height, actual_size, - _high_sensitivity, true ); + _gyro_scale_factor, true ); } else { @@ -225,21 +225,26 @@ namespace librealsense gyroscope_transform::gyroscope_transform( std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity ) - : gyroscope_transform( "Gyroscope Transform", mm_calib, mm_correct_opt, high_sensitivity ) + double gyro_scale_factor ) + : gyroscope_transform( "Gyroscope Transform", mm_calib, mm_correct_opt, gyro_scale_factor ) {} gyroscope_transform::gyroscope_transform( const char * name, std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity ) + double gyro_scale_factor ) : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, mm_calib, mm_correct_opt) - , _high_sensitivity( high_sensitivity ) + , _gyro_scale_factor( gyro_scale_factor ) {} void gyroscope_transform::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, int actual_size ) { - unpack_gyro_axes< RS2_FORMAT_MOTION_XYZ32F >( dest, source, width, height, actual_size, _high_sensitivity ); + unpack_gyro_axes< RS2_FORMAT_MOTION_XYZ32F >( dest, + source, + width, + height, + actual_size, + _gyro_scale_factor ); } } diff --git a/src/proc/motion-transform.h b/src/proc/motion-transform.h index 5d631f8eb8..8813722bb1 100644 --- a/src/proc/motion-transform.h +++ b/src/proc/motion-transform.h @@ -40,20 +40,20 @@ namespace librealsense public: motion_to_accel_gyro( std::shared_ptr< mm_calib_handler > mm_calib = nullptr, std::shared_ptr< enable_motion_correction > mm_correct_opt = nullptr, - bool high_sensitivity = false ); + double gyro_scale_factor = 0.1 ); protected: motion_to_accel_gyro( const char * name, std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity = false ); + double gyro_scale_factor ); void configure_processing_callback(); void process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size ) override; void correct_motion(float3* xyz) const; std::shared_ptr _source_stream_profile; std::shared_ptr _accel_gyro_target_profile; - bool _high_sensitivity = false; + double _gyro_scale_factor = 0.1; }; class acceleration_transform : public motion_transform @@ -72,16 +72,16 @@ namespace librealsense public: gyroscope_transform( std::shared_ptr< mm_calib_handler > mm_calib = nullptr, std::shared_ptr< enable_motion_correction > mm_correct_opt = nullptr, - bool high_sensitivity = false ); + double gyro_scale_factor = 0.1 ); protected: gyroscope_transform( const char * name, std::shared_ptr< mm_calib_handler > mm_calib, std::shared_ptr< enable_motion_correction > mm_correct_opt, - bool high_sensitivity = false ); + double gyro_scale_factor ); void process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size ) override; - bool _high_sensitivity = false; + double _gyro_scale_factor = 0.1; }; } From 9e42d09d5b008188e6df45423049cfc924773e8e Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 12 Mar 2024 16:53:48 +0200 Subject: [PATCH 16/21] adding set_gyro_sensitivity function implementation --- src/linux/backend-hid.h | 2 ++ src/platform/hid-device.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/linux/backend-hid.h b/src/linux/backend-hid.h index 7ff4cc1033..bc70bfd644 100644 --- a/src/linux/backend-hid.h +++ b/src/linux/backend-hid.h @@ -215,6 +215,8 @@ namespace librealsense static void foreach_hid_device(std::function action); + void set_gyro_scale_factor( double scale_factor ) override{}; + private: static bool get_hid_device_info(const char* dev_path, hid_device_info& device_info); diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index 99507a0fb1..d7f20e0630 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -124,6 +124,8 @@ class multi_pins_hid_device : public hid_device return _dev.front()->get_custom_report_data( custom_sensor_name, report_name, report_field ); } + void set_gyro_scale_factor( double scale_factor ) override {}; + private: std::vector< std::shared_ptr< hid_device > > _dev; std::vector< hid_profile > _hid_profiles; From 6200401ac76dcadad6330671b7ac8591344f7183 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 12 Mar 2024 17:24:05 +0200 Subject: [PATCH 17/21] add set_gyro_scale function --- src/hid/hid-device.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/hid/hid-device.h b/src/hid/hid-device.h index d4a3752a33..bd5a9df595 100644 --- a/src/hid/hid-device.h +++ b/src/hid/hid-device.h @@ -43,6 +43,7 @@ namespace librealsense virtual std::vector get_custom_report_data(const std::string& custom_sensor_name, const std::string& report_name, custom_sensor_report_field report_field) override { return {}; } + void set_gyro_scale_factor( double scale_factor ) {} private: void handle_interrupt(); From fe843e1c80a74c5606f69c3bbb4292d65d688813 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 12 Mar 2024 17:25:26 +0200 Subject: [PATCH 18/21] add ; --- src/hid/hid-device.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hid/hid-device.h b/src/hid/hid-device.h index bd5a9df595..630c645c7a 100644 --- a/src/hid/hid-device.h +++ b/src/hid/hid-device.h @@ -43,7 +43,7 @@ namespace librealsense virtual std::vector get_custom_report_data(const std::string& custom_sensor_name, const std::string& report_name, custom_sensor_report_field report_field) override { return {}; } - void set_gyro_scale_factor( double scale_factor ) {} + void set_gyro_scale_factor( double scale_factor ) override{}; private: void handle_interrupt(); From d301900d531842a6b5a13f864931dd6ec4147b75 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Sun, 17 Mar 2024 13:18:00 +0200 Subject: [PATCH 19/21] add D457 support for new FW version and fix PR comments --- src/ds/d400/d400-motion.cpp | 9 ++++++++- src/ds/d400/d400-options.cpp | 12 ++++++------ src/ds/ds-motion-common.cpp | 1 + src/ds/features/gyro-sensitivity-feature.cpp | 1 - src/hid-sensor.cpp | 4 ++-- src/hid-sensor.h | 2 +- src/mf/mf-hid.cpp | 1 + src/mf/mf-hid.h | 1 + src/platform/hid-data.h | 9 +++------ src/platform/hid-device.h | 3 --- src/proc/motion-transform.cpp | 2 -- 11 files changed, 23 insertions(+), 22 deletions(-) diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index 9f7dd68820..21f8bb51f5 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -80,10 +80,12 @@ namespace librealsense } catch (...) {} + double gyro_scale_factor = _fw_version >= firmware_version( 5, 15, 1, 224 ) ? 0.0001 : 0.1 ; + motion_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F} }, { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL}, {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, - [&, mm_correct_opt]() { return std::make_shared(_mm_calib, mm_correct_opt, false); + [&, mm_correct_opt]() { return std::make_shared(_mm_calib, mm_correct_opt, gyro_scale_factor); }); return motion_ep; @@ -129,8 +131,13 @@ namespace librealsense // HID metadata attributes hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&hid_header::timestamp)); } + //for FW >=5.16 the scale factor changes to 1000.0 since FW sends 32bit + if (_fw_version >= firmware_version( 5, 15, 1, 224)) + get_raw_motion_sensor()->set_gyro_scale_factor( 10000.0 ); + } + ds_motion_sensor & d400_motion::get_motion_sensor() { return dynamic_cast< ds_motion_sensor & >( get_sensor( _motion_module_device_idx.value() ) ); diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index 0686f3f356..ce956ac104 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -495,19 +495,19 @@ namespace librealsense switch( static_cast< int >( val ) ) { case RS2_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC: { - return "61.0"; + return "61.0 mDeg/Sec"; } case RS2_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC: { - return "30.5"; + return "30.5 mDeg/Sec"; } case RS2_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC: { - return "15.3"; + return "15.3 mDeg/Sec"; } case RS2_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC: { - return "7.6"; + return "7.6 mDeg/Sec"; } case RS2_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC: { - return "3.8"; + return "3.8 mDeg/Sec"; } default: throw invalid_value_exception( "value not found" ); @@ -516,7 +516,7 @@ namespace librealsense const char * librealsense::gyro_sensitivity_option::get_description() const { - return "gyro sensitivity resolutions"; + return "gyro sensitivity resolutions, lowers the dynamic range for a more accurate readings"; } bool librealsense::gyro_sensitivity_option::is_read_only() const diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 385bc5359b..557dd1317a 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -513,6 +513,7 @@ namespace librealsense [&, mm_correct_opt]() { return std::make_shared< acceleration_transform >( _mm_calib, mm_correct_opt ); }); + //TODO this FW version is relevant for d400 devices. Need to change for propre d500 devices support. bool high_sensitivity = _owner->is_gyro_high_sensitivity(); double gyro_scale_factor = high_sensitivity ? 0.003814697265625 : ( _fw_version >= firmware_version( 5, 15, 1, 224 ) ? 0.0001: 0.1 ); hid_ep->register_processing_block( diff --git a/src/ds/features/gyro-sensitivity-feature.cpp b/src/ds/features/gyro-sensitivity-feature.cpp index ab48a144d1..b09c717260 100644 --- a/src/ds/features/gyro-sensitivity-feature.cpp +++ b/src/ds/features/gyro-sensitivity-feature.cpp @@ -18,7 +18,6 @@ gyro_sensitivity_feature::gyro_sensitivity_feature( std::shared_ptr< hid_sensor option_range enable_range = { 0.f /*min*/, 4.f /*max*/, 1.f /*step*/, 1.f /*default*/ }; auto imu_sensitivity_control = std::make_shared< gyro_sensitivity_option >( motion_sensor, enable_range ); motion.register_option( RS2_OPTION_GYRO_SENSITIVITY, imu_sensitivity_control ); - motion_sensor->set_gyro_scale_factor( 10000.0 ); } diff --git a/src/hid-sensor.cpp b/src/hid-sensor.cpp index 274a4f48cd..f6798fa200 100644 --- a/src/hid-sensor.cpp +++ b/src/hid-sensor.cpp @@ -367,8 +367,8 @@ double hid_sensor::get_imu_sensitivity_values( rs2_stream stream ) return gyro_sensitivity_convert.at( _imu_sensitivity_per_rs2_stream[stream] ); } else - //gyro sensitivity default value is +-1000 therefore sending 0.1 - //accel sensitivity default value is +-4g therefore sending 0.001 + //FW recieve 0.1 and adjusts the gyro's sensitivity to its default setting of ±1000. + //FW recieve 0.001 and adjusts the accel's sensitivity to its default setting of ±4g. return stream == RS2_STREAM_GYRO ? 0.1f : 0.001f; } diff --git a/src/hid-sensor.h b/src/hid-sensor.h index cc4ca6908f..255a91d442 100644 --- a/src/hid-sensor.h +++ b/src/hid-sensor.h @@ -71,7 +71,7 @@ class hid_sensor : public raw_sensor_base std::vector< platform::hid_sensor > _hid_sensors; std::unique_ptr< frame_timestamp_reader > _hid_iio_timestamp_reader; std::unique_ptr< frame_timestamp_reader > _custom_hid_timestamp_reader; - //this map is intended to keep the values for gyro and accel sensitivity if the user changed them, available from FW >=5.16 + //Keeps set sensitivity values for gyro and accel std::map< rs2_stream, float > _imu_sensitivity_per_rs2_stream; stream_profiles get_sensor_profiles( std::string sensor_name ) const; diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index 919cff1ee3..c6261e9a35 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -167,6 +167,7 @@ namespace librealsense CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &var)); rawZ = var.dblVal; + //TODO: get scale factor from HID descriptor,then setting it from the feature will not be needed. rawX *= _gyro_scale_factor; rawY *= _gyro_scale_factor; rawZ *= _gyro_scale_factor; diff --git a/src/mf/mf-hid.h b/src/mf/mf-hid.h index 9f84d4a6f6..d2dd6d81b0 100644 --- a/src/mf/mf-hid.h +++ b/src/mf/mf-hid.h @@ -79,6 +79,7 @@ namespace librealsense CComPtr _cb; std::vector _hid_profiles; + //10.0 was used for D400 before FW support to gyro sensitivity control double _gyro_scale_factor = 10.0; }; } diff --git a/src/platform/hid-data.h b/src/platform/hid-data.h index b4cbd71404..caa7e523b2 100644 --- a/src/platform/hid-data.h +++ b/src/platform/hid-data.h @@ -12,11 +12,8 @@ namespace librealsense { struct hid_data { int32_t x; - //uint16_t reserved1[2]; int32_t y; - //uint16_t reserved2[2]; int32_t z; - //uint16_t reserved3[2]; }; @@ -25,9 +22,9 @@ struct hid_mipi_data uint8_t typeID; uint8_t skip1; uint64_t hwTs; - int16_t x; - int16_t y; - int16_t z; + int32_t x; + int32_t y; + int32_t z; uint64_t hwTs2; uint64_t skip2; }; diff --git a/src/platform/hid-device.h b/src/platform/hid-device.h index d7f20e0630..8e3fc9c2bd 100644 --- a/src/platform/hid-device.h +++ b/src/platform/hid-device.h @@ -45,11 +45,8 @@ struct sensor_data struct hid_sensor_data { int32_t x; - //char reserved1[2]; int32_t y; - //char reserved2[2]; int32_t z; - //char reserved3[2]; uint32_t ts_low; uint32_t ts_high; }; diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp index 0248bf8864..67a575b9e5 100644 --- a/src/proc/motion-transform.cpp +++ b/src/proc/motion-transform.cpp @@ -48,8 +48,6 @@ namespace librealsense void unpack_gyro_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, double gyro_scale_factor = 0.1, bool is_mipi = false ) { - // Default sensitivity in HKR is +-2000 deg/sec at 16.384 LSB/Deg/Sec. - // High sensitivity is +-125 deg/sec at 262.144 LSB/Deg/Sec (LSB is 0.003814697265625 deg/sec), in legacy the calculations happen in FW and we need scalefactor 0.1. const double gyro_transform_factor = deg2rad( gyro_scale_factor ); copy_hid_axes(dest, source, gyro_transform_factor, is_mipi); From 95585522fb8fb3da647a6ff1d71bc6af0c737255 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Sun, 17 Mar 2024 14:38:26 +0200 Subject: [PATCH 20/21] add gyro_scale_factor param --- src/ds/d400/d400-motion.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index 21f8bb51f5..0ff85cab75 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -85,7 +85,8 @@ namespace librealsense motion_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F} }, { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL}, {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, - [&, mm_correct_opt]() { return std::make_shared(_mm_calib, mm_correct_opt, gyro_scale_factor); + [&, mm_correct_opt, gyro_scale_factor]() + { return std::make_shared< motion_to_accel_gyro >( _mm_calib, mm_correct_opt, gyro_scale_factor ); }); return motion_ep; From 89ec1362294585aa0e7b0e5adaac39f276a58e1e Mon Sep 17 00:00:00 2001 From: noacoohen Date: Sun, 17 Mar 2024 19:04:03 +0200 Subject: [PATCH 21/21] final PR fixes, changed the FW version to 5.16 --- src/ds/d400/d400-factory.cpp | 4 ++-- src/ds/d400/d400-motion.cpp | 3 ++- src/ds/ds-motion-common.cpp | 2 +- src/mf/mf-hid.cpp | 1 - 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index b12eefc108..3f1d877510 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -672,7 +672,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { check_and_restore_rgb_stream_extrinsic(); - if( _fw_version >= firmware_version( 5, 15, 1, 224 ) ) + if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } @@ -1009,7 +1009,7 @@ namespace librealsense dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) , d400_thermal_tracking( d400_device::_thermal_monitor ) { - if( _fw_version >= firmware_version( 5, 15, 1, 224 ) ) + if( _fw_version >= firmware_version( 5, 16, 0, 0 ) ) register_feature( std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) ); } diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp index 0ff85cab75..760f6008f9 100644 --- a/src/ds/d400/d400-motion.cpp +++ b/src/ds/d400/d400-motion.cpp @@ -80,7 +80,8 @@ namespace librealsense } catch (...) {} - double gyro_scale_factor = _fw_version >= firmware_version( 5, 15, 1, 224 ) ? 0.0001 : 0.1 ; + // For FW >=5.16 the scale factor changed to 0.0001 to support higher resolution (diff between two adjacent samples) + double gyro_scale_factor = _fw_version >= firmware_version( 5, 16, 0, 0 ) ? 0.0001 : 0.1 ; motion_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F} }, diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp index 557dd1317a..053ce786da 100644 --- a/src/ds/ds-motion-common.cpp +++ b/src/ds/ds-motion-common.cpp @@ -515,7 +515,7 @@ namespace librealsense //TODO this FW version is relevant for d400 devices. Need to change for propre d500 devices support. bool high_sensitivity = _owner->is_gyro_high_sensitivity(); - double gyro_scale_factor = high_sensitivity ? 0.003814697265625 : ( _fw_version >= firmware_version( 5, 15, 1, 224 ) ? 0.0001: 0.1 ); + double gyro_scale_factor = high_sensitivity ? 0.003814697265625 : ( _fw_version >= firmware_version( 5, 16, 0, 0 ) ? 0.0001: 0.1 ); hid_ep->register_processing_block( { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} }, diff --git a/src/mf/mf-hid.cpp b/src/mf/mf-hid.cpp index c6261e9a35..919cff1ee3 100644 --- a/src/mf/mf-hid.cpp +++ b/src/mf/mf-hid.cpp @@ -167,7 +167,6 @@ namespace librealsense CHECK_HR(report->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &var)); rawZ = var.dblVal; - //TODO: get scale factor from HID descriptor,then setting it from the feature will not be needed. rawX *= _gyro_scale_factor; rawY *= _gyro_scale_factor; rawZ *= _gyro_scale_factor;