PR IntelRealSense#12715 from noacoohen: Gyro sensitivity feature

include/librealsense2/h/rs_option.h

@@ -123,6 +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, 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;
 
@@ -274,6 +275,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_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 );
+
     /**
     * check if an option is read-only
     * \param[in] options  the options container

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, GYRO_SENSITIVITY )
 
 
 }  // namespace librealsense

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/gyro-sensitivity-feature.h"
+        "${CMAKE_CURRENT_LIST_DIR}/features/gyro-sensitivity-feature.cpp"
 )

src/ds/d400/d400-factory.cpp

@@ -33,6 +33,8 @@
 
 #include <src/ds/features/auto-exposure-limit-feature.h>
 #include <src/ds/features/gain-limit-feature.h>
+#include <src/ds/features/gyro-sensitivity-feature.h>
+
 namespace librealsense
 {
     // PSR
@@ -670,6 +672,9 @@ 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 ) )
+                register_feature(
+                    std::make_shared< gyro_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, 16, 0, 0 ) )
+                register_feature(
+                    std::make_shared< gyro_sensitivity_feature >( get_raw_motion_sensor(), get_motion_sensor() ) );
         }
 
         std::shared_ptr<matcher> create_matcher(const frame_holder& frame) const override;

src/ds/d400/d400-motion.cpp

@@ -21,7 +21,7 @@
 #include "proc/auto-exposure-processor.h"
 #include <src/fourcc.h>
 #include <src/metadata-parser.h>
-
+#include <src/hid-sensor.h>
 using namespace librealsense;
 namespace librealsense
 {
@@ -80,10 +80,14 @@ namespace librealsense
         }
         catch (...) {}
 
+        // 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} },
             { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL}, {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
-            [&, mm_correct_opt]() { return std::make_shared<motion_to_accel_gyro>(_mm_calib, mm_correct_opt, false);
+            [&, 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;
@@ -129,6 +133,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));
         }
+        //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() ) );
+    }
+
+    std::shared_ptr<hid_sensor> d400_motion::get_raw_motion_sensor()
+    {
+        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 )

src/ds/d400/d400-motion.h

@@ -47,6 +47,8 @@ namespace librealsense
         std::shared_ptr<synthetic_sensor> create_hid_device(std::shared_ptr<context> ctx,
             const std::vector<platform::hid_device_info>& all_hid_infos,
             const firmware_version& camera_fw_version);
+        ds_motion_sensor & get_motion_sensor();
+        std::shared_ptr<hid_sensor > get_raw_motion_sensor();
 
     protected:
         friend class ds_motion_common;

src/ds/d400/d400-options.cpp

@@ -6,7 +6,7 @@
 #include "d400-options.h"
 
 #include <rsutils/string/from.h>
-
+#include <src/hid-sensor.h>
 
 namespace librealsense
 {
@@ -466,4 +466,66 @@ namespace librealsense
     {
         snapshot = std::make_shared<const_value_option>(get_description(), 0.f);
     }
-}
+
+    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 ) );
+
+        _value = value;
+        strong->set_imu_sensitivity( RS2_STREAM_GYRO, value );
+    }
+
+    float librealsense::gyro_sensitivity_option::query() const
+    {
+        return _value;
+    }
+
+
+    const char * librealsense::gyro_sensitivity_option::get_value_description( float val ) const
+    {
+        switch( static_cast< int >( val ) )
+        {
+            case RS2_GYRO_SENSITIVITY_61_0_MILLI_DEG_SEC: {
+                return "61.0 mDeg/Sec";
+            }
+            case RS2_GYRO_SENSITIVITY_30_5_MILLI_DEG_SEC: {
+                return "30.5 mDeg/Sec";
+            }
+            case RS2_GYRO_SENSITIVITY_15_3_MILLI_DEG_SEC: {
+                return "15.3 mDeg/Sec";
+            }
+            case RS2_GYRO_SENSITIVITY_7_6_MILLI_DEG_SEC: {
+                return "7.6 mDeg/Sec";
+            }
+            case RS2_GYRO_SENSITIVITY_3_8_MILLI_DEG_SEC: {
+                return "3.8 mDeg/Sec";
+            }
+            default:
+                throw invalid_value_exception( "value not found" );
+        }
+    }
+
+    const char * librealsense::gyro_sensitivity_option::get_description() const
+    {
+        return "gyro sensitivity resolutions, lowers the dynamic range for a more accurate readings";
+    }
+
+    bool librealsense::gyro_sensitivity_option::is_read_only() const
+    {
+        if( auto strong = _sensor.lock() )
+            return strong->is_opened();
+        return false;
+    }
+
+
+ }
+

src/ds/d400/d400-options.h

@@ -188,4 +188,35 @@ namespace librealsense
 
         std::function<void(const option&)> _recording_function = [](const option&) {};
     };
+
+
+    class hid_sensor;
+    class gyro_sensitivity_option: public option_base
+    {
+    public:
+        gyro_sensitivity_option( const std::weak_ptr< hid_sensor > & sensor, const option_range & opt_range )
+            : option_base( opt_range )
+            , _value( opt_range.def )
+            , _sensor( sensor )
+        {
+            set( _value );
+        }
+        virtual ~gyro_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:
+        float _value;
+        std::weak_ptr< hid_sensor > _sensor;
+        std::function< void( const option & ) > _record_action = []( const option & ) {};
+
+    };
 }

src/ds/ds-motion-common.cpp

@@ -513,12 +513,14 @@ 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, 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} },
-            [&, 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;

src/ds/features/gyro-sensitivity-feature.cpp

@@ -0,0 +1,29 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
+
+#include <src/ds/features/gyro-sensitivity-feature.h>
+#include <src/ds/d400/d400-options.h>
+#include <src/ds/ds-private.h>
+#include <src/sensor.h>
+#include <librealsense2/hpp/rs_options.hpp>
+
+
+namespace librealsense {
+
+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*/, 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 );
+}
+
+
+feature_id gyro_sensitivity_feature::get_id() const
+{
+    return ID;
+}
+
+}//namespace librealsense

src/ds/features/gyro-sensitivity-feature.h

@@ -0,0 +1,29 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
+
+#pragma once
+
+
+#include <src/feature-interface.h>
+#include <src/hid-sensor.h>
+#include <src/ds/ds-motion-common.h>
+
+#include <memory>
+
+namespace librealsense {
+
+
+class gyro_sensitivity_feature : public feature_interface
+{
+
+public:
+    static const feature_id ID;
+
+    gyro_sensitivity_feature( std::shared_ptr< hid_sensor > motion_sensor, ds_motion_sensor & motion );
+
+    feature_id get_id() const override;
+
+
+};
+
+}  // namespace librealsense

src/hid-sensor.cpp

@@ -20,6 +20,14 @@ 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 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 } };
 
     // in sensor.cpp
 void log_callback_end( uint32_t fps,
@@ -113,7 +121,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_values( request->get_stream_type() ) } );
     }
     _hid_device->open( configured_hid_profiles );
     if( Is< librealsense::global_time_interface >( _owner ) )
@@ -339,6 +348,29 @@ uint32_t hid_sensor::fps_to_sampling_frequency( rs2_stream stream, uint32_t fps
     else
         return fps;
 }
+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 )
+{
+    if( _imu_sensitivity_per_rs2_stream.find( stream ) != _imu_sensitivity_per_rs2_stream.end() )
+    {
+        return gyro_sensitivity_convert.at( _imu_sensitivity_per_rs2_stream[stream] );
+    }
+    else
+        //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;
+}
 
 iio_hid_timestamp_reader::iio_hid_timestamp_reader()
 {

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( 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;
@@ -69,6 +71,8 @@ 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;
+    //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;