PR #12965 from noacoohen: IMU frame index jump in D457-Mipi

src/uvc-sensor.cpp

@@ -32,6 +32,8 @@ uvc_sensor::uvc_sensor( std::string const & name,
     , _device( std::move( uvc_device ) )
     , _user_count( 0 )
     , _timestamp_reader( std::move( timestamp_reader ) )
+    , _gyro_counter(0)
+    , _accel_counter(0)
 {
     register_metadata( RS2_FRAME_METADATA_BACKEND_TIMESTAMP,
                        make_additional_data_parser( &frame_additional_data::backend_timestamp ) );
@@ -133,22 +135,33 @@ void uvc_sensor::open( const stream_profiles & requests )
                         return;
                     }
 
-                    const auto && fr = generate_frame_from_data( f,
+                    auto && fr = generate_frame_from_data( f,
                                                                  system_time,
                                                                  _timestamp_reader.get(),
                                                                  last_timestamp,
                                                                  last_frame_number,
                                                                  req_profile_base );
-                    const auto && timestamp_domain = _timestamp_reader->get_frame_timestamp_domain( fr );
+                    auto timestamp_domain = _timestamp_reader->get_frame_timestamp_domain( fr );
                     auto bpp = get_image_bpp( req_profile_base->get_format() );
-                    auto && frame_counter = fr->additional_data.frame_number;
-                    auto && timestamp = fr->additional_data.timestamp;
+                    auto & frame_counter = fr->additional_data.frame_number;
+                    auto & timestamp = fr->additional_data.timestamp;
 
                     // D457 development
                     size_t expected_size;
                     auto && msp = As< motion_stream_profile, stream_profile_interface >( req_profile );
                     if( msp )
+                    {
                         expected_size = 64;  // 32; // D457 - WORKAROUND - SHOULD BE REMOVED AFTER CORRECTION IN DRIVER
+                        //Motion stream on uvc is used only for mipi. Stream frame number counts gyro and accel together.
+                        //We override it using 2 seperate counters.
+                        auto stream_type = ((uint8_t *)f.pixels)[0];
+                        if( stream_type == 1 ) // 1 == Accel
+                            fr->additional_data.frame_number = ++_accel_counter;
+                        else if( stream_type == 2 ) // 2 == Gyro
+                            fr->additional_data.frame_number = ++_gyro_counter;
+                        frame_counter = fr->additional_data.frame_number;
+                    }
+
 
                     LOG_DEBUG( "FrameAccepted,"
                                << librealsense::get_string( req_profile_base->get_stream_type() ) << ",Counter,"
@@ -385,6 +398,8 @@ void uvc_sensor::stop()
     _is_streaming = false;
     _device->stop_callbacks();
     _timestamp_reader->reset();
+    _gyro_counter = 0;
+    _accel_counter = 0;
     raise_on_before_streaming_changes( false );
 }
 

src/uvc-sensor.h

@@ -65,6 +65,9 @@ class uvc_sensor : public raw_sensor_base
     void acquire_power();
     void release_power();
     void reset_streaming();
+    std::atomic<int64_t> _gyro_counter;
+    std::atomic<int64_t> _accel_counter;
+
 
     struct power
     {

unit-tests/live/d400/test-mipi-motion.py

@@ -0,0 +1,38 @@
+# License: Apache 2.0. See LICENSE file in root directory.
+# Copyright(c) 2024 Intel Corporation. All Rights Reserved.
+
+# test:device:jetson D457
+# test:donotrun:!jetson 
+
+import pyrealsense2 as rs
+from rspy import test
+import time
+gyro_frame_count = 0
+accel_frame_count = 0
+
+def frame_callback( f ):
+    global gyro_frame_count,accel_frame_count
+    stream_type = f.get_profile().stream_type()
+    if stream_type == rs.stream.gyro:
+        gyro_frame_count += 1
+        test.check_equal(f.get_frame_number(),gyro_frame_count)
+    elif stream_type == rs.stream.accel:
+        accel_frame_count += 1
+        test.check_equal(f.get_frame_number(), accel_frame_count)
+
+################################################################################################
+with test.closure("frame index - mipi IMU "):
+    seconds_to_count_frames = 10
+    dev = test.find_first_device_or_exit()
+    sensor = dev.first_motion_sensor()
+    motion_profile_accel = next(p for p in sensor.profiles if p.stream_type() == rs.stream.accel and p.fps() == 100)
+    motion_profile_gyro = next(p for p in sensor.profiles if p.stream_type() == rs.stream.gyro and p.fps()==100)
+    sensor.open( [motion_profile_accel, motion_profile_gyro] )
+    sensor.start( frame_callback )
+    time.sleep(seconds_to_count_frames) # Time to count frames
+    sensor.stop()
+    sensor.close()
+    test.check(gyro_frame_count > 0)
+    test.check(accel_frame_count > 0)
+
+test.print_results_and_exit()