Skip to content

Commit ee15237

Browse files
Kontra2BKontra2B
committed
mipi ready
1 parent 88ecb03 commit ee15237

10 files changed

Lines changed: 110 additions & 134 deletions

File tree

‎examples/capture/rs-capture.cpp‎

Lines changed: 24 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
77
#include <map>
88
#include "example.hpp" // Include short list of convenience functions for rendering
9+
#include "librealsense2/h/rs_types.h"
910

1011
// Capture Example demonstrates how to
1112
// capture depth and color video streams and render them to the screen
@@ -17,9 +18,9 @@ int main(int argc, char * argv[]) try
1718
// window app(1280, 720, "RealSense Capture Example");
1819

1920
// Declare depth colorizer for pretty visualization of depth data
20-
rs2::colorizer color_map;
21+
// rs2::colorizer color_map;
2122
// Declare rates printer for showing streaming rates of the enabled streams.
22-
rs2::rates_printer printer;
23+
// rs2::rates_printer printer;
2324

2425
// Declare RealSense pipeline, encapsulating the actual device and sensors
2526
rs2::pipeline pipe;
@@ -42,41 +43,50 @@ int main(int argc, char * argv[]) try
4243
int idx = 0;
4344
while (true) // Application still alive?
4445
{
45-
rs2::frameset data = pipe.wait_for_frames(4000000000).
46-
apply_filter(printer). // Print each enabled stream frame rate
47-
apply_filter(color_map); // Find and colorize the depth data
46+
rs2::frameset data = pipe.wait_for_frames();
47+
// rs2::frameset data = pipe.wait_for_frames(4000000000).apply_filter(printer). // Print each enabled stream frame rate
48+
// apply_filter(color_map); // Find and colorize the depth data
4849
// std::cout << std::endl << "----- CLOCK:" << std::chrono::steady_clock::now().time_since_epoch().count() << "ns" << std::endl;
4950
const size_t nf = data.size();
5051

51-
std::cout << std::endl << idx << '/' << nf << '.';
52-
data.foreach_rs([&last, &idx ] (const rs2::frame& f) {
52+
rs2_metadata_type tmin = 0.0, tmax = 0.0;
53+
// std::cout << std::endl << "WOJTEK: " << tmin << '/' << tmax << ' ' << sizeof(tmin);
54+
std::cout << std::endl << idx << '/' << nf << '.' << std::fixed << std::setprecision(3);
55+
data.foreach_rs([&last, &idx, &tmin, &tmax] (const rs2::frame& f) {
5356
static int ne = 0;
5457
auto fn = f.get_profile().stream_name().substr(0, 1);
5558
auto fi = f.get_profile().stream_index();
5659
auto fu = f.get_profile().unique_id();
57-
rs2_metadata_type fc, ft;
60+
rs2_metadata_type ufc;
61+
rs2_metadata_type ft;
62+
// rs2_time_t ft;
5863
try {
59-
fc = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER);
64+
ufc = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER);
6065
ft = f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP);
66+
// ft = f.get_timestamp();
6167
} catch(...) { return; }
6268
// auto fr = f.get_frame_metadata(RS2_FRAME_METADATA_CRC);
6369
//char path[64];
6470
//sprintf(path, "S%d-%d.bin", (int)fs, (int)idx);
65-
std::cout << '\t' << fn << fu << ':' << fc << '/' << ft;
71+
std::cout << '\t' << fn << fu << ':' << ufc << '/' << ft;
6672
try {
6773
const auto& ofs = last.at(fu);
68-
if (!(fc > ofs.first && ft > ofs.second)) {
74+
if (!(ufc > ofs.first && ft > ofs.second)) {
6975
std::cout << " !!!/" << ++ne << ' ' << fn << ':' << ofs.first << '/' << ofs.second;
7076
if (ne > 10) exit(EXIT_FAILURE);
7177
}
72-
else if (fc > ofs.first + 1)
73-
std::cout << " +";
78+
else if (ufc > ofs.first + 1)
79+
std::cout << ' ' << '+' << ufc - ofs.first -1;
80+
std::cout << '/' << ft - ofs.second;
7481
} catch(...) {}
75-
last[fu] = {fc, ft};
82+
if (ft > tmax) tmax = ft;
83+
if (tmin == 0.0 || ft < tmin) tmin = ft;
84+
last[fu] = {ufc, ft};
7685
//auto fo = fopen(path, "wb+");
7786
//fwrite(fd, f.get_data_size(), 1, fo);
7887
//fclose(fo);
7988
});
89+
std::cout << '\t' << '\t' << tmin << '/' << tmax << '/' << tmax - tmin;
8090
idx++;
8191

8292
// The show method, when applied on frameset, break it to frames and upload each frame into a gl textures

‎src/ds/ds-timestamp.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,9 @@ namespace librealsense
6161
auto md = (librealsense::metadata_intel_basic*)(f->additional_data.metadata_blob.data());
6262
if(_has_metadata[pin_index] && md)
6363
{
64-
return (double)(md->header.timestamp)*TIMESTAMP_USEC_TO_MSEC;
64+
auto time = ((double)md->header.timestamp)*TIMESTAMP_USEC_TO_MSEC;
65+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << md->header.timestamp << '/' << std::fixed << time);
66+
return time;
6567
}
6668
else
6769
{

‎src/global_timestamp_reader.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -297,6 +297,7 @@ namespace librealsense
297297
{
298298
double frame_time = _device_timestamp_reader->get_frame_timestamp(frame);
299299
rs2_timestamp_domain ts_domain = _device_timestamp_reader->get_frame_timestamp_domain(frame);
300+
auto raw = frame_time;
300301
if (_option_is_enabled->is_true() && ts_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK)
301302
{
302303
auto sp = _time_diff_keeper.lock();
@@ -305,6 +306,7 @@ namespace librealsense
305306
else
306307
LOG_DEBUG("Notification: global_timestamp_reader - time_diff_keeper is being shut-down");
307308
}
309+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << std::fixed << raw << '/' << frame_time);
308310
return frame_time;
309311
}
310312

‎src/linux/backend-v4l2.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ constexpr bool metadata_node = false;
6363
#define V4L2_META_FMT_D4XX v4l2_fourcc('D', '4', 'X', 'X') /* D400 Payload Header metadata */
6464
#endif
6565

66-
#undef DEBUG_V4L
66+
#define DEBUG_V4L
6767
#ifdef DEBUG_V4L
6868
#define LOG_DEBUG_V4L(...) do { CLOG(DEBUG ,LIBREALSENSE_ELPP_ID) << __VA_ARGS__; } while(false)
6969
#else

‎src/pipeline/aggregator.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ namespace librealsense
5757
std::vector<frame_holder> async_set;
5858
for (auto&& s : _last_set)
5959
{
60+
if (!s.second) continue;
6061
sync_set.push_back(s.second.clone());
6162
// send only the synchronized frames to the user callback
6263
if (std::find(_streams_to_sync_ids.begin(), _streams_to_sync_ids.end(),

‎src/sensor.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -328,6 +328,7 @@ void log_callback_end( uint32_t fps,
328328

329329
if (_metadata_modifier)
330330
_metadata_modifier(additional_data);
331+
fr->additional_data = additional_data;
331332

332333
// update additional data
333334
additional_data.timestamp = timestamp_reader->get_frame_timestamp(fr);

‎src/small-heap.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ class small_heap
3030
for( auto i = 0; i < C; i++ )
3131
{
3232
is_free[i] = true;
33-
buffer[i] = std::move( T() );
33+
// buffer[i] = std::move( T() );
3434
}
3535
}
3636

‎src/sync.cpp‎

Lines changed: 59 additions & 93 deletions
Original file line numberDiff line numberDiff line change
@@ -149,17 +149,6 @@ namespace librealsense
149149
_name = create_composite_name(matchers, name);
150150
}
151151

152-
composite_matcher::matcher_queue::matcher_queue()
153-
: q( QUEUE_MAX_SIZE,
154-
[]( frame_holder const & fh )
155-
{
156-
// If queues are overrun, we'll get here
157-
LOG_DEBUG( "DROPPED frame " << fh );
158-
} )
159-
{
160-
}
161-
162-
163152
void composite_matcher::dispatch(frame_holder f, const syncronization_environment& env)
164153
{
165154
clean_inactive_streams(f);
@@ -193,10 +182,7 @@ namespace librealsense
193182
if( matcher )
194183
{
195184
if( ! matcher->get_active() )
196-
{
197185
matcher->set_active( true );
198-
_frames_queue[matcher.get()].q.start();
199-
}
200186
return matcher;
201187
}
202188
}
@@ -231,9 +217,7 @@ namespace librealsense
231217
for (auto stream : matcher->get_streams())
232218
{
233219
if (_matchers[stream])
234-
{
235-
_frames_queue.erase(_matchers[stream].get());
236-
}
220+
_frames.erase(_matchers[stream].get());
237221
_matchers[stream] = matcher;
238222
_streams_id.push_back(stream);
239223
}
@@ -287,10 +271,6 @@ namespace librealsense
287271
// Mark ourselves inactive, so we don't get new dispatches
288272
set_active( false );
289273

290-
// Stop all our queues to wake up anyone waiting on them
291-
for( auto & fq : _frames_queue )
292-
fq.second.q.stop();
293-
294274
// Trickle the stop down to any children
295275
for( auto m : _matchers )
296276
m.second->stop();
@@ -310,90 +290,76 @@ namespace librealsense
310290
{
311291
std::ostringstream os;
312292
os << '[';
313-
for( auto m : matchers )
314-
{
315-
auto const & q = _frames_queue[m].q;
316-
q.peek( [&os]( frame_holder const & fh ) {
317-
os << fh;
318-
} );
319-
}
293+
for(auto m : matchers)
294+
os << *_frames[m];
320295
os << ']';
321296
return os.str();
322297
}
323298

324299
void composite_matcher::sync(frame_holder f, const syncronization_environment& env)
325300
{
326-
auto matcher = find_matcher(f);
327-
if (!matcher)
328-
{
329-
LOG_ERROR("didn't find any matcher for " << f << " will not be synchronized");
330-
_callback(std::move(f), env);
331-
return;
332-
}
333-
update_next_expected( matcher, f );
334-
335-
if( ! _frames_queue[matcher.get()].q.enqueue( std::move( f ) ) )
336-
// If we get stopped, nothing to do!
337-
return;
301+
// We don't want to stop while syncing!
302+
std::lock_guard< std::mutex > lock( _mutex );
338303

339-
// We have a queue for each known stream we want to sync.
340-
// E.g., for (Depth Color), we need to sync two frames, one from each.
341-
// If we have a Color frame but not Depth, then Depth is "missing" and needs to be
342-
// waited-for...
343-
344-
std::map<librealsense::matcher *, frame_holder> frames_arrived;
345-
std::vector< int > synced_frames;
346-
std::vector< int > unsynced_frames;
347-
std::vector< librealsense::matcher * > missing_streams;
348-
std::vector< frame_holder > match;
349-
350-
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << *f.frame);
351-
while( true )
352-
{
304+
auto matcher = find_matcher(f);
305+
if (!matcher)
353306
{
354-
// We don't want to stop while syncing!
355-
std::lock_guard< std::mutex > lock( _mutex );
356-
357-
// We want to release one frame from each matcher. If a matcher has nothing queued, it is "missing" and
358-
// we need to consider waiting for it:
359-
for( auto s = _frames_queue.begin(); s != _frames_queue.end(); s++ )
360-
if (s->second.q.empty()) return;
307+
LOG_ERROR("didn't find any matcher for " << f << " will not be synchronized");
308+
_callback(std::move(f), env);
309+
return;
310+
}
311+
update_next_expected( matcher, f );
361312

362-
for( auto s = _frames_queue.begin(); s != _frames_queue.end(); s++ )
363-
{
364-
librealsense::matcher * const m = s->first;
365-
while(s->second.q.try_dequeue(&frames_arrived[m]))
366-
;
367-
}
313+
if (_frames[matcher.get()])
314+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << "!new frame" << *_frames[matcher.get()].frame << " < " << *f.frame);
315+
else
316+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << "new frame " << *f.frame);
317+
_frames[matcher.get()] = std::move(f);
318+
319+
// We have a queue for each known stream we want to sync.
320+
// E.g., for (Depth Color), we need to sync two frames, one from each.
321+
// If we have a Color frame but not Depth, then Depth is "missing" and needs to be
322+
// waited-for...
323+
324+
std::vector< frame_holder > match;
325+
rs2_time_t tref = 0.0, tmax = 0.0;
326+
327+
// We want to release one frame from each matcher. If a matcher has nothing queued, it is "missing" and
328+
// we need to consider waiting for it:
329+
for(auto& entry: _frames) {
330+
if (!entry.second) {
331+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << "missing " << _name);
332+
return;
333+
}
334+
tref += entry.second.frame->get_frame_timestamp();
335+
auto fps = entry.second.frame->get_stream()->get_framerate();
336+
if (1.0/fps > tmax) tmax = 1.0/fps;
337+
}
368338

369-
// From what we collected, we want to release only the frames that are synchronized (based on timestamp,
370-
// number, etc.) -- anything else we'll leave to the next iteration. The synced frames should be the
371-
// earliest possible!
339+
tref /= _frames.size();
340+
tmax *= 1000/2.0/10.0;
372341

373-
for(auto& matches: frames_arrived)
374-
match.push_back(std::move(matches.second));
342+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << std::fixed << tref << '/' << tmax);
343+
for(auto& entry: _frames) {
344+
auto diff = tref - entry.second.frame->get_frame_timestamp();
345+
if (diff > tmax) {
346+
LOG_DEBUG("[WOJTEK] " << __func__ << '/' << diff << ':' << *entry.second << " !too old");
347+
entry.second.reset();
348+
return;
349+
}
350+
LOG_DEBUG("[WOJTEK] " << __func__ << '/' << diff << ':' << *entry.second);
375351
}
376352

377-
// The frameset should always be with the same order of streams (the first stream carries extra
378-
// meaning because it decides the frameset properties) -- so we sort them...
379-
if (match.size() > 1 ) {
380-
std::sort( match.begin(),
381-
match.end(),
382-
[]( const frame_holder & f1, const frame_holder & f2 ) {
383-
return f1.frame->get_stream()->get_unique_id()
384-
> f2.frame->get_stream()->get_unique_id();
385-
} );
386-
}
353+
for(auto& entry: _frames)
354+
match.push_back(std::move(entry.second));
387355

388356
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
389357
if (composite.frame)
390358
{
391-
begin_callback();
392-
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << *composite.frame);
393-
_callback(std::move(composite), env);
359+
begin_callback();
360+
LOG_DEBUG("[WOJTEK] " << __func__ << ':' << *composite.frame);
361+
_callback(std::move(composite), env);
394362
}
395-
break;
396-
}
397363
}
398364

399365
frame_number_composite_matcher::frame_number_composite_matcher(
@@ -439,7 +405,7 @@ namespace librealsense
439405

440406
for(auto id: inactive_matchers)
441407
{
442-
_frames_queue[_matchers[id].get()].q.clear();
408+
_frames[_matchers[id].get()].reset();
443409
}
444410
}
445411

@@ -468,7 +434,7 @@ namespace librealsense
468434
_next_expected[matcher.get()].value = f.frame->get_frame_number()+1.;
469435
}
470436

471-
std::pair<double, double> extract_timestamps(frame_holder & a, frame_holder & b)
437+
std::pair<rs2_time_t, rs2_time_t> extract_timestamps(frame_holder & a, frame_holder & b)
472438
{
473439
if (a->get_frame_timestamp_domain() == b->get_frame_timestamp_domain())
474440
return{ a->get_frame_timestamp(), b->get_frame_timestamp() };
@@ -639,11 +605,11 @@ namespace librealsense
639605
<< rsutils::string::from( next_expected.value + threshold ) << "; deactivating matcher!",
640606
env );
641607

642-
auto const q_it = _frames_queue.find( missing );
643-
if( q_it != _frames_queue.end() )
608+
auto const q_it = _frames.find( missing );
609+
if( q_it != _frames.end() )
644610
{
645-
if( q_it->second.q.empty() )
646-
_frames_queue.erase( q_it );
611+
if(!q_it->second)
612+
_frames.erase(q_it);
647613
}
648614
missing->set_active( false );
649615
return true;

‎src/sync.h‎

Lines changed: 2 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@
1313
#include <memory>
1414
#include <map>
1515

16-
1716
namespace librealsense {
1817

1918

@@ -110,17 +109,9 @@ namespace librealsense {
110109

111110
protected:
112111
virtual void update_next_expected( std::shared_ptr< matcher > const & matcher,
113-
const frame_holder & f )
114-
= 0;
115-
116-
struct matcher_queue
117-
{
118-
single_consumer_frame_queue< frame_holder > q;
119-
120-
matcher_queue();
121-
};
112+
const frame_holder & f ) = 0;
122113

123-
std::map< matcher *, matcher_queue > _frames_queue;
114+
std::map<matcher *, frame_holder> _frames;
124115
std::map<stream_id, std::shared_ptr<matcher>> _matchers;
125116
struct next_expected_t
126117
{

0 commit comments

Comments
 (0)