PR IntelRealSense#12358 from OhadMeir: Gyro sensitivity might not be …

…const for all models Gyro sensitivity might not be const for all models
Nir-Az · Nov 5, 2023 · 24bdec8 · 24bdec8
2 parents 83af55f + 30f98e7
commit 24bdec8
Show file tree

Hide file tree

Showing 7 changed files with 59 additions and 18 deletions.
diff --git a/src/backend-device.h b/src/backend-device.h
@@ -31,6 +31,8 @@ class backend_device : public virtual device
     uint16_t get_pid() const { return _pid; }
     std::shared_ptr< platform::backend > get_backend();
 
+    virtual bool is_gyro_high_sensitivity() const { return false; }
+
 protected:
     uint16_t _pid = 0;
 };

diff --git a/src/ds/d400/d400-motion.cpp b/src/ds/d400/d400-motion.cpp
@@ -81,7 +81,7 @@ 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<motion_to_accel_gyro>(_mm_calib, mm_correct_opt);
+            [&, mm_correct_opt]() { return std::make_shared<motion_to_accel_gyro>(_mm_calib, mm_correct_opt, false);
         });
 
         return motion_ep;

diff --git a/src/ds/d500/d500-motion.cpp b/src/ds/d500/d500-motion.cpp
@@ -29,6 +29,11 @@ namespace librealsense
         return _ds_motion_common->get_motion_intrinsics(stream);
     }
 
+    bool d500_motion::is_gyro_high_sensitivity() const
+    {
+        return false;
+    }
+
     std::shared_ptr<synthetic_sensor> d500_motion::create_hid_device(std::shared_ptr<context> ctx,
                                                                 const std::vector<platform::hid_device_info>& all_hid_infos,
                                                                 const firmware_version& camera_fw_version)

diff --git a/src/ds/d500/d500-motion.h b/src/ds/d500/d500-motion.h
@@ -19,6 +19,8 @@ namespace librealsense
 
         rs2_motion_device_intrinsic get_motion_intrinsics(rs2_stream) const;
 
+        bool is_gyro_high_sensitivity() const override;
+
     protected:
         friend class ds_motion_common;
         friend class ds_fisheye_sensor;

diff --git a/src/ds/ds-motion-common.cpp b/src/ds/ds-motion-common.cpp
@@ -504,13 +504,15 @@ namespace librealsense
         hid_ep->register_processing_block(
             { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
             { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
-            [&, mm_correct_opt]() { return std::make_shared<acceleration_transform>(_mm_calib, mm_correct_opt);
+            [&, mm_correct_opt]() { return std::make_shared< acceleration_transform >( _mm_calib, mm_correct_opt );
             });
 
+        bool high_sensitivity = _owner->is_gyro_high_sensitivity();
         hid_ep->register_processing_block(
             { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
             { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
-            [&, mm_correct_opt]() { return std::make_shared<gyroscope_transform>(_mm_calib, mm_correct_opt);
+            [&, mm_correct_opt, high_sensitivity]() {
+                return std::make_shared< gyroscope_transform >( _mm_calib, mm_correct_opt, high_sensitivity );
             });
 
         return hid_ep;

diff --git a/src/proc/motion-transform.cpp b/src/proc/motion-transform.cpp
@@ -39,11 +39,15 @@ namespace librealsense
         copy_hid_axes<FORMAT>(dest, source, accelerator_transform_factor, is_mipi);
     }
 
-    // The Gyro input format: signed int 16bit. data units 1LSB=0.1deg/sec;
+    // The Gyro input format: signed int 16bit. data units depends on set sensitivity;
     // Librealsense output format: floating point 32bit. units rad/sec,
-    template<rs2_format FORMAT> void unpack_gyro_axes(byte * const dest[], const byte * source, int width, int height, int output_size, bool is_mipi = false)
+    template< rs2_format FORMAT >
+    void unpack_gyro_axes( byte * const dest[], const byte * source, int width, int height, int output_size,
+                           bool high_sensitivity = false, bool is_mipi = false )
     {
-        static const double gyro_transform_factor = deg2rad(0.1);
+        // 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).
+        const double gyro_transform_factor = deg2rad( high_sensitivity ? 0.003814697265625 : 0.1 );
 
         copy_hid_axes<FORMAT>(dest, source, gyro_transform_factor, is_mipi);
     }
@@ -116,12 +120,18 @@ namespace librealsense
         correct_motion_helper(xyz, _accel_gyro_target_profile->get_stream_type());
     }
 
-    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)
-        : motion_to_accel_gyro("Accel_Gyro Transform", mm_calib, mm_correct_opt)
+    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 )
     {}
 
-    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)
+    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 )
         : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ANY, mm_calib, mm_correct_opt)
+        , _high_sensitivity( high_sensitivity )
     {
         configure_processing_callback();
     }
@@ -189,7 +199,8 @@ namespace librealsense
         else if (source[0] == 2)
         {
             _target_stream = RS2_STREAM_GYRO;
-            unpack_gyro_axes<RS2_FORMAT_MOTION_XYZ32F>(dest, source, width, height, actual_size, true);
+            unpack_gyro_axes<RS2_FORMAT_MOTION_XYZ32F>( dest, source, width, height, actual_size,
+                                                        _high_sensitivity, true );
         }
         else
         {
@@ -210,17 +221,23 @@ namespace librealsense
         unpack_accel_axes<RS2_FORMAT_MOTION_XYZ32F>(dest, source, width, height, actual_size);
     }
 
-    gyroscope_transform::gyroscope_transform(std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
-        : gyroscope_transform("Gyroscope Transform", mm_calib, mm_correct_opt)
+    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 )
     {}
 
-    gyroscope_transform::gyroscope_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
+    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 )
         : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, mm_calib, mm_correct_opt)
+        , _high_sensitivity( high_sensitivity )
     {}
 
     void gyroscope_transform::process_function(byte * const dest[], const byte * source, int width, int height, int output_size, int actual_size)
     {
-        unpack_gyro_axes<RS2_FORMAT_MOTION_XYZ32F>(dest, source, width, height, actual_size);
+        unpack_gyro_axes< RS2_FORMAT_MOTION_XYZ32F >( dest, source, width, height, actual_size, _high_sensitivity );
     }
 }
 
diff --git a/src/proc/motion-transform.h b/src/proc/motion-transform.h
@@ -38,16 +38,22 @@ namespace librealsense
     class motion_to_accel_gyro : public motion_transform
     {
     public:
-        motion_to_accel_gyro(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr);
+        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 );
 
     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);
+        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 );
         void configure_processing_callback();
         void process_function(byte * const dest[], const byte * source, int width, int height, int actual_size, int input_size) override;
         void correct_motion(float3* xyz) const;
 
         std::shared_ptr<stream_profile_interface> _source_stream_profile;
         std::shared_ptr<stream_profile_interface> _accel_gyro_target_profile;
+        bool _high_sensitivity = false;
     };
 
     class acceleration_transform : public motion_transform
@@ -64,10 +70,17 @@ namespace librealsense
     class gyroscope_transform : public motion_transform
     {
     public:
-        gyroscope_transform(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr);
+        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 );
 
     protected:
-        gyroscope_transform(const char* name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt);
+        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 );
         void process_function(byte * const dest[], const byte * source, int width, int height, int actual_size, int input_size) override;
+
+        bool _high_sensitivity = false;
     };
 }