diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index 9da7006146..847647f736 100644 --- a/src/uvc-sensor.cpp +++ b/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 ); } diff --git a/src/uvc-sensor.h b/src/uvc-sensor.h index 0b21a1d3fc..f2370d56b0 100644 --- a/src/uvc-sensor.h +++ b/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 _gyro_counter; + std::atomic _accel_counter; + struct power { diff --git a/unit-tests/live/d400/test-mipi-motion.py b/unit-tests/live/d400/test-mipi-motion.py new file mode 100644 index 0000000000..6c9028d522 --- /dev/null +++ b/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() \ No newline at end of file