diff --git a/src/global_timestamp_reader.cpp b/src/global_timestamp_reader.cpp index 706c4d5c9..0886fcfd7 100644 --- a/src/global_timestamp_reader.cpp +++ b/src/global_timestamp_reader.cpp @@ -136,7 +136,7 @@ namespace librealsense base_x = -max_device_time; else return false; - LOG_DEBUG(__FUNCTION__ << "(" << base_x << ")"); + LOG_INFO(__FUNCTION__ << "(" << base_x << ")"); double a, b; get_a_b(x+base_x, a, b); @@ -281,13 +281,46 @@ namespace librealsense double global_timestamp_reader::get_frame_timestamp(const std::shared_ptr& frame) { + static const int arr_size = 2000; + static double arr[arr_size]; + static int idx = 0; + double now = duration(system_clock::now().time_since_epoch()).count(); double frame_time = _device_timestamp_reader->get_frame_timestamp(frame); rs2_timestamp_domain ts_domain = _device_timestamp_reader->get_frame_timestamp_domain(frame); if (_option_is_enabled->is_true() && ts_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { auto sp = _time_diff_keeper.lock(); if (sp) + { frame_time = sp->get_system_hw_time(frame_time, _ts_is_ready); + if (frame_time >= now) + LOG_ERROR("Future Timestamp!"); + arr[idx++] = now - frame_time; + if (idx >= arr_size) + { + double sum = 0; + double min = arr[0]; + double max = arr[0]; + + for (int i = 0; i < arr_size; i++) + { + sum += arr[i]; + min = std::min(min, arr[i]); + max = std::max(max, arr[i]); + } + + double mean = sum / arr_size; + + double sq_sum = 0; + for (int i = 0; i < arr_size; i++) + sq_sum += (arr[i] - mean) * (arr[i] - mean); + + double stdev = std::sqrt(sq_sum / (arr_size - 1)); + + LOG_INFO("HW Timestamp Interval (ms): mean=" << mean << " min=" << min << " max=" << max << " stdev=" << stdev); + idx = 0; + } + } else LOG_DEBUG("Notification: global_timestamp_reader - time_diff_keeper is being shut-down"); }