From b7e62f76f2cb5604c182cecf3830814a540b1dd6 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 27 May 2024 20:29:52 +0300 Subject: [PATCH 1/9] add fix for imu index frame jump in mipi --- src/uvc-sensor.cpp | 21 +++++++++++++++++---- src/uvc-sensor.h | 3 +++ 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index 9da7006146..8f0e843e93 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,31 @@ 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 + auto pixels = (uint8_t *)f.pixels; + if( pixels[0] == 1 ) + fr->additional_data.frame_number = ++_accel_counter; + else if( pixels[0] == 2 ) + 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 +396,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 { From 4e88d15fb3d96b85565f3402fd97ab5f10933df6 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 28 May 2024 09:02:42 +0300 Subject: [PATCH 2/9] add comment --- src/uvc-sensor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index 8f0e843e93..c6f7c9027b 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -152,6 +152,7 @@ void uvc_sensor::open( const stream_profiles & requests ) if( msp ) { expected_size = 64; // 32; // D457 - WORKAROUND - SHOULD BE REMOVED AFTER CORRECTION IN DRIVER + //For D457 motion stream frame number counts gyro and accel together. We override it using 2 seperate counters. auto pixels = (uint8_t *)f.pixels; if( pixels[0] == 1 ) fr->additional_data.frame_number = ++_accel_counter; From df07c34d56514dec58721599abd89073dba06399 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 28 May 2024 09:32:21 +0300 Subject: [PATCH 3/9] edit comment --- src/uvc-sensor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index c6f7c9027b..f43c58c74f 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -152,7 +152,8 @@ void uvc_sensor::open( const stream_profiles & requests ) if( msp ) { expected_size = 64; // 32; // D457 - WORKAROUND - SHOULD BE REMOVED AFTER CORRECTION IN DRIVER - //For D457 motion stream frame number counts gyro and accel together. We override it using 2 seperate counters. + //Motion stran on uvc is used only for mipi. Stream frame number counts gyro and accel together. + //We override it using 2 seperate counters. auto pixels = (uint8_t *)f.pixels; if( pixels[0] == 1 ) fr->additional_data.frame_number = ++_accel_counter; From e32606bc16bd02a14cdfe0c558d81af2c114f5a8 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Tue, 28 May 2024 11:17:00 +0300 Subject: [PATCH 4/9] fix typo --- src/uvc-sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index f43c58c74f..8a554e6060 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -152,7 +152,7 @@ void uvc_sensor::open( const stream_profiles & requests ) if( msp ) { expected_size = 64; // 32; // D457 - WORKAROUND - SHOULD BE REMOVED AFTER CORRECTION IN DRIVER - //Motion stran on uvc is used only for mipi. Stream frame number counts gyro and accel together. + //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 pixels = (uint8_t *)f.pixels; if( pixels[0] == 1 ) From f42a00855a617436ded0ea8c63505acc83504b7f Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 29 May 2024 13:57:11 +0300 Subject: [PATCH 5/9] add unit test --- unit-tests/live/d400/test-mipi-motion.py | 35 ++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 unit-tests/live/d400/test-mipi-motion.py 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..4dd8621c81 --- /dev/null +++ b/unit-tests/live/d400/test-mipi-motion.py @@ -0,0 +1,35 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +# test:device:jetson D457 + +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.print_results_and_exit() \ No newline at end of file From 59bb9f5dcc3a375ad917be6aa652218845796dcc Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 29 May 2024 14:39:29 +0300 Subject: [PATCH 6/9] add checks to make sure frames arrived --- unit-tests/live/d400/test-mipi-motion.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/unit-tests/live/d400/test-mipi-motion.py b/unit-tests/live/d400/test-mipi-motion.py index 4dd8621c81..8df9e04381 100644 --- a/unit-tests/live/d400/test-mipi-motion.py +++ b/unit-tests/live/d400/test-mipi-motion.py @@ -31,5 +31,7 @@ def frame_callback( f ): 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 From 78c9247a86b6f7aad151f5c73f4aa4ffe1670297 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 29 May 2024 15:06:13 +0300 Subject: [PATCH 7/9] add donotrun if it is not jetson --- unit-tests/live/d400/test-mipi-motion.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unit-tests/live/d400/test-mipi-motion.py b/unit-tests/live/d400/test-mipi-motion.py index 8df9e04381..6c9028d522 100644 --- a/unit-tests/live/d400/test-mipi-motion.py +++ b/unit-tests/live/d400/test-mipi-motion.py @@ -2,6 +2,7 @@ # Copyright(c) 2024 Intel Corporation. All Rights Reserved. # test:device:jetson D457 +# test:donotrun:!jetson import pyrealsense2 as rs from rspy import test @@ -33,5 +34,5 @@ def frame_callback( f ): 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 From c3d601977a5e56d013c45dfd2502b4195742d8ec Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 29 May 2024 15:15:41 +0300 Subject: [PATCH 8/9] add comments and chane name of the variable --- src/uvc-sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index 8a554e6060..b6d6de21df 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -154,10 +154,10 @@ void uvc_sensor::open( const stream_profiles & requests ) 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 pixels = (uint8_t *)f.pixels; - if( pixels[0] == 1 ) + auto stream_type = (uint8_t *)f.pixels; + if( stream_type[0] == 1 ) // 1 == Accel fr->additional_data.frame_number = ++_accel_counter; - else if( pixels[0] == 2 ) + else if( stream_type[0] == 2 ) // 2 == Gyro fr->additional_data.frame_number = ++_gyro_counter; frame_counter = fr->additional_data.frame_number; } From e84c78d4d8c44eb5ae62c16d0c3dda112d0e770d Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 29 May 2024 15:49:21 +0300 Subject: [PATCH 9/9] set stream_type hold the value itself --- src/uvc-sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/uvc-sensor.cpp b/src/uvc-sensor.cpp index b6d6de21df..847647f736 100644 --- a/src/uvc-sensor.cpp +++ b/src/uvc-sensor.cpp @@ -154,10 +154,10 @@ void uvc_sensor::open( const stream_profiles & requests ) 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; - if( stream_type[0] == 1 ) // 1 == Accel + 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[0] == 2 ) // 2 == Gyro + else if( stream_type == 2 ) // 2 == Gyro fr->additional_data.frame_number = ++_gyro_counter; frame_counter = fr->additional_data.frame_number; }