fix imu data

Author: noacoohen

src/ds/d400/d400-motion.cpp

@@ -41,6 +41,19 @@ namespace librealsense
         return _ds_motion_common->get_motion_intrinsics(stream);
     }
 
+    bool d400_motion_base::is_imu_high_accuracy() const
+    {
+        // D400 FW 5.16 and above use 32 bits in the struct, instead of 16.
+        return _fw_version >= firmware_version( 5, 16, 0, 0 ); 
+    }
+
+    double d400_motion_base::get_gyro_default_scale() const
+    {
+        // FW scale in the HID feature report was 10 up to 5.16, changed to 1000 to support gyro sensitivity option.
+        // D400 FW performs conversion from raw to physical, we get [deg/sec] values.
+        return _fw_version >= firmware_version( 5, 16, 0, 0 ) ? 0.0001 : 0.1;
+    }
+
     std::shared_ptr<synthetic_sensor> d400_motion_uvc::create_uvc_device(std::shared_ptr<context> ctx,
                                                   const std::vector<platform::uvc_device_info>& all_uvc_infos,
                                                   const firmware_version& camera_fw_version)
@@ -88,7 +101,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, gyro_scale_factor]()
+            [&, high_accuracy, mm_correct_opt, gyro_scale_factor]()
             { return std::make_shared< motion_to_accel_gyro >( _mm_calib, mm_correct_opt, gyro_scale_factor, high_accuracy );
         });
 

src/ds/d400/d400-motion.h

@@ -13,6 +13,8 @@ namespace librealsense
     {
     public:
         rs2_motion_device_intrinsic get_motion_intrinsics(rs2_stream) const;
+        bool is_imu_high_accuracy() const override;
+        double get_gyro_default_scale() const override;
 
         std::shared_ptr<auto_exposure_mechanism> register_auto_exposure_options(synthetic_sensor* ep,
             const platform::extension_unit* fisheye_xu);

src/proc/motion-transform.cpp

@@ -166,10 +166,19 @@ namespace librealsense
                                                 bool high_accuracy )
         : motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ANY, mm_calib, mm_correct_opt)
     {
+        static constexpr float gravity = 9.80665f;  // Standard Gravitation Acceleration
+        static constexpr double accelerator_scale_factor = 0.001 * gravity;
+
         if( high_accuracy )
+        {
             _converter = std::make_unique< converter_32_bit_mipi >( deg2rad( gyro_scale_factor ) );
+            _accel_converter = std::make_unique< converter_32_bit_mipi >( accelerator_scale_factor );
+        }
         else
+        {
             _converter = std::make_unique< converter_16_bit_mipi >( deg2rad( gyro_scale_factor ) );
+            _accel_converter = std::make_unique< converter_32_bit_mipi >( accelerator_scale_factor );
+        }
         configure_processing_callback();
     }
 
@@ -230,17 +239,18 @@ namespace librealsense
         if (source[0] == 1)
         {
             _target_stream = RS2_STREAM_ACCEL;
+            _accel_converter->convert( dest, source );
         }
         else if (source[0] == 2)
         {
             _target_stream = RS2_STREAM_GYRO;
+            _converter->convert( dest, source );
         }
         else
         {
             throw("motion_to_accel_gyro::process_function - stream type not discovered");
         }
 
-        _converter->convert( dest, source );
     }
 
     acceleration_transform::acceleration_transform( std::shared_ptr< mm_calib_handler > mm_calib,

src/proc/motion-transform.h

@@ -67,6 +67,7 @@ namespace librealsense
 
         std::shared_ptr<stream_profile_interface> _source_stream_profile;
         std::shared_ptr<stream_profile_interface> _accel_gyro_target_profile;
+        std::unique_ptr< imu_to_librs_converter > _accel_converter;
     };
 
     class acceleration_transform : public motion_transform