diff --git a/BUILD.gn b/BUILD.gn index aaa1260088..52736b325a 100644 --- a/BUILD.gn +++ b/BUILD.gn @@ -574,6 +574,7 @@ if (rtc_include_tests && !build_with_chromium) { "rtc_base/task_utils:pending_task_safety_flag_unittests", "rtc_base/task_utils:repeating_task_unittests", "rtc_base/task_utils:to_queued_task_unittests", + "rtc_base/time:timestamp_extrapolator_unittests", "rtc_base/units:units_unittests", "sdk:sdk_tests", "test:rtp_test_utils", diff --git a/rtc_base/time/BUILD.gn b/rtc_base/time/BUILD.gn index 890695dfeb..092abd9a83 100644 --- a/rtc_base/time/BUILD.gn +++ b/rtc_base/time/BUILD.gn @@ -17,6 +17,26 @@ rtc_library("timestamp_extrapolator") { "timestamp_extrapolator.cc", "timestamp_extrapolator.h", ] - deps = [ "../../api/units:timestamp" ] + deps = [ + "../../api/units:frequency", + "../../api/units:timestamp", + "../../modules:module_api_public", + ] absl_deps = [ "//third_party/abseil-cpp/absl/types:optional" ] } + +if (rtc_include_tests) { + rtc_library("timestamp_extrapolator_unittests") { + testonly = true + sources = [ "timestamp_extrapolator_unittest.cc" ] + deps = [ + ":timestamp_extrapolator", + "../../api/units:frequency", + "../../api/units:time_delta", + "../../api/units:timestamp", + "../../system_wrappers", + "../../test:test_support", + ] + absl_deps = [ "//third_party/abseil-cpp/absl/types:optional" ] + } +} diff --git a/rtc_base/time/timestamp_extrapolator.cc b/rtc_base/time/timestamp_extrapolator.cc index 521b5ba4c4..910db8a390 100644 --- a/rtc_base/time/timestamp_extrapolator.cc +++ b/rtc_base/time/timestamp_extrapolator.cc @@ -13,87 +13,83 @@ #include #include "absl/types/optional.h" +#include "api/units/frequency.h" +#include "modules/include/module_common_types_public.h" namespace webrtc { +namespace { + +constexpr double kLambda = 1; +constexpr uint32_t kStartUpFilterDelayInPackets = 2; +constexpr double kAlarmThreshold = 60e3; +// in timestamp ticks, i.e. 15 ms +constexpr double kAccDrift = 6600; +constexpr double kAccMaxError = 7000; +constexpr double kP11 = 1e10; + +} // namespace + TimestampExtrapolator::TimestampExtrapolator(Timestamp start) - : _start(Timestamp::Zero()), - _prev(Timestamp::Zero()), - _firstTimestamp(0), - _wrapArounds(0), - _prevUnwrappedTimestamp(-1), - _prevWrapTimestamp(-1), - _lambda(1), - _firstAfterReset(true), - _packetCount(0), - _startUpFilterDelayInPackets(2), - _detectorAccumulatorPos(0), - _detectorAccumulatorNeg(0), - _alarmThreshold(60e3), - _accDrift(6600), // in timestamp ticks, i.e. 15 ms - _accMaxError(7000), - _pP11(1e10) { + : start_(Timestamp::Zero()), + prev_(Timestamp::Zero()), + packet_count_(0), + detector_accumulator_pos_(0), + detector_accumulator_neg_(0) { Reset(start); } void TimestampExtrapolator::Reset(Timestamp start) { - _start = start; - _prev = _start; - _firstTimestamp = 0; - _w[0] = 90.0; - _w[1] = 0; - _pP[0][0] = 1; - _pP[1][1] = _pP11; - _pP[0][1] = _pP[1][0] = 0; - _firstAfterReset = true; - _prevUnwrappedTimestamp = -1; - _prevWrapTimestamp = -1; - _wrapArounds = 0; - _packetCount = 0; - _detectorAccumulatorPos = 0; - _detectorAccumulatorNeg = 0; + start_ = start; + prev_ = start_; + first_unwrapped_timestamp_ = absl::nullopt; + w_[0] = 90.0; + w_[1] = 0; + p_[0][0] = 1; + p_[1][1] = kP11; + p_[0][1] = p_[1][0] = 0; + unwrapper_ = TimestampUnwrapper(); + packet_count_ = 0; + detector_accumulator_pos_ = 0; + detector_accumulator_neg_ = 0; } void TimestampExtrapolator::Update(Timestamp now, uint32_t ts90khz) { - if (now - _prev > TimeDelta::Seconds(10)) { + if (now - prev_ > TimeDelta::Seconds(10)) { // Ten seconds without a complete frame. // Reset the extrapolator Reset(now); } else { - _prev = now; + prev_ = now; } // Remove offset to prevent badly scaled matrices - const TimeDelta offset = now - _start; - double tMs = offset.ms(); + const TimeDelta offset = now - start_; + double t_ms = offset.ms(); - CheckForWrapArounds(ts90khz); + int64_t unwrapped_ts90khz = unwrapper_.Unwrap(ts90khz); - int64_t unwrapped_ts90khz = - static_cast(ts90khz) + - _wrapArounds * ((static_cast(1) << 32) - 1); - - if (_firstAfterReset) { + if (!first_unwrapped_timestamp_) { // Make an initial guess of the offset, - // should be almost correct since tMs - _startMs + // should be almost correct since t_ms - start // should about zero at this time. - _w[1] = -_w[0] * tMs; - _firstTimestamp = unwrapped_ts90khz; - _firstAfterReset = false; + w_[1] = -w_[0] * t_ms; + first_unwrapped_timestamp_ = unwrapped_ts90khz; } - double residual = (static_cast(unwrapped_ts90khz) - _firstTimestamp) - - tMs * _w[0] - _w[1]; + double residual = + (static_cast(unwrapped_ts90khz) - *first_unwrapped_timestamp_) - + t_ms * w_[0] - w_[1]; if (DelayChangeDetection(residual) && - _packetCount >= _startUpFilterDelayInPackets) { + packet_count_ >= kStartUpFilterDelayInPackets) { // A sudden change of average network delay has been detected. // Force the filter to adjust its offset parameter by changing // the offset uncertainty. Don't do this during startup. - _pP[1][1] = _pP11; + p_[1][1] = kP11; } - if (_prevUnwrappedTimestamp >= 0 && - unwrapped_ts90khz < _prevUnwrappedTimestamp) { + if (prev_unwrapped_timestamp_ && + unwrapped_ts90khz < prev_unwrapped_timestamp_) { // Drop reordered frames. return; } @@ -102,94 +98,62 @@ void TimestampExtrapolator::Update(Timestamp now, uint32_t ts90khz) { // that = T'*w; // K = P*T/(lambda + T'*P*T); double K[2]; - K[0] = _pP[0][0] * tMs + _pP[0][1]; - K[1] = _pP[1][0] * tMs + _pP[1][1]; - double TPT = _lambda + tMs * K[0] + K[1]; + K[0] = p_[0][0] * t_ms + p_[0][1]; + K[1] = p_[1][0] * t_ms + p_[1][1]; + double TPT = kLambda + t_ms * K[0] + K[1]; K[0] /= TPT; K[1] /= TPT; // w = w + K*(ts(k) - that); - _w[0] = _w[0] + K[0] * residual; - _w[1] = _w[1] + K[1] * residual; + w_[0] = w_[0] + K[0] * residual; + w_[1] = w_[1] + K[1] * residual; // P = 1/lambda*(P - K*T'*P); double p00 = - 1 / _lambda * (_pP[0][0] - (K[0] * tMs * _pP[0][0] + K[0] * _pP[1][0])); + 1 / kLambda * (p_[0][0] - (K[0] * t_ms * p_[0][0] + K[0] * p_[1][0])); double p01 = - 1 / _lambda * (_pP[0][1] - (K[0] * tMs * _pP[0][1] + K[0] * _pP[1][1])); - _pP[1][0] = - 1 / _lambda * (_pP[1][0] - (K[1] * tMs * _pP[0][0] + K[1] * _pP[1][0])); - _pP[1][1] = - 1 / _lambda * (_pP[1][1] - (K[1] * tMs * _pP[0][1] + K[1] * _pP[1][1])); - _pP[0][0] = p00; - _pP[0][1] = p01; - _prevUnwrappedTimestamp = unwrapped_ts90khz; - if (_packetCount < _startUpFilterDelayInPackets) { - _packetCount++; + 1 / kLambda * (p_[0][1] - (K[0] * t_ms * p_[0][1] + K[0] * p_[1][1])); + p_[1][0] = + 1 / kLambda * (p_[1][0] - (K[1] * t_ms * p_[0][0] + K[1] * p_[1][0])); + p_[1][1] = + 1 / kLambda * (p_[1][1] - (K[1] * t_ms * p_[0][1] + K[1] * p_[1][1])); + p_[0][0] = p00; + p_[0][1] = p01; + prev_unwrapped_timestamp_ = unwrapped_ts90khz; + if (packet_count_ < kStartUpFilterDelayInPackets) { + packet_count_++; } } absl::optional TimestampExtrapolator::ExtrapolateLocalTime( - uint32_t timestamp90khz) { - CheckForWrapArounds(timestamp90khz); - double unwrapped_ts90khz = - static_cast(timestamp90khz) + - _wrapArounds * ((static_cast(1) << 32) - 1); - if (_packetCount == 0) { - return absl::nullopt; - } else if (_packetCount < _startUpFilterDelayInPackets) { - auto diffMs = static_cast( - static_cast(unwrapped_ts90khz - _prevUnwrappedTimestamp) / - 90.0 + - 0.5); - return _prev + TimeDelta::Millis(diffMs); - } else if (_w[0] < 1e-3) { - return _start; - } else { - double timestampDiff = - unwrapped_ts90khz - static_cast(_firstTimestamp); - auto diffMs = static_cast((timestampDiff - _w[1]) / _w[0] + 0.5); - return _start + TimeDelta::Millis(diffMs); - } -} + uint32_t timestamp90khz) const { + int64_t unwrapped_ts90khz = unwrapper_.UnwrapWithoutUpdate(timestamp90khz); -// Investigates if the timestamp clock has overflowed since the last timestamp -// and keeps track of the number of wrap arounds since reset. -void TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz) { - if (_prevWrapTimestamp == -1) { - _prevWrapTimestamp = ts90khz; - return; - } - if (ts90khz < _prevWrapTimestamp) { - // This difference will probably be less than -2^31 if we have had a wrap - // around (e.g. timestamp = 1, _previousTimestamp = 2^32 - 1). Since it is - // casted to a Word32, it should be positive. - if (static_cast(ts90khz - _prevWrapTimestamp) > 0) { - // Forward wrap around - _wrapArounds++; - } + if (!first_unwrapped_timestamp_) { + return absl::nullopt; + } else if (packet_count_ < kStartUpFilterDelayInPackets) { + constexpr Frequency k90KHz = Frequency::KiloHertz(90); + TimeDelta diff = (unwrapped_ts90khz - *prev_unwrapped_timestamp_) / k90KHz; + return prev_ + diff; + } else if (w_[0] < 1e-3) { + return start_; } else { - // This difference will probably be less than -2^31 if we have had a - // backward wrap around. Since it is casted to a Word32, it should be - // positive. - if (static_cast(_prevWrapTimestamp - ts90khz) > 0) { - // Backward wrap around - _wrapArounds--; - } + double timestampDiff = unwrapped_ts90khz - *first_unwrapped_timestamp_; + auto diff_ms = static_cast((timestampDiff - w_[1]) / w_[0] + 0.5); + return start_ + TimeDelta::Millis(diff_ms); } - _prevWrapTimestamp = ts90khz; } bool TimestampExtrapolator::DelayChangeDetection(double error) { // CUSUM detection of sudden delay changes - error = (error > 0) ? std::min(error, _accMaxError) - : std::max(error, -_accMaxError); - _detectorAccumulatorPos = - std::max(_detectorAccumulatorPos + error - _accDrift, double{0}); - _detectorAccumulatorNeg = - std::min(_detectorAccumulatorNeg + error + _accDrift, double{0}); - if (_detectorAccumulatorPos > _alarmThreshold || - _detectorAccumulatorNeg < -_alarmThreshold) { + error = (error > 0) ? std::min(error, kAccMaxError) + : std::max(error, -kAccMaxError); + detector_accumulator_pos_ = + std::max(detector_accumulator_pos_ + error - kAccDrift, double{0}); + detector_accumulator_neg_ = + std::min(detector_accumulator_neg_ + error + kAccDrift, double{0}); + if (detector_accumulator_pos_ > kAlarmThreshold || + detector_accumulator_neg_ < -kAlarmThreshold) { // Alarm - _detectorAccumulatorPos = _detectorAccumulatorNeg = 0; + detector_accumulator_pos_ = detector_accumulator_neg_ = 0; return true; } return false; diff --git a/rtc_base/time/timestamp_extrapolator.h b/rtc_base/time/timestamp_extrapolator.h index df56eea986..23e7975453 100644 --- a/rtc_base/time/timestamp_extrapolator.h +++ b/rtc_base/time/timestamp_extrapolator.h @@ -15,6 +15,7 @@ #include "absl/types/optional.h" #include "api/units/timestamp.h" +#include "modules/include/module_common_types_public.h" namespace webrtc { @@ -23,31 +24,23 @@ class TimestampExtrapolator { public: explicit TimestampExtrapolator(Timestamp start); void Update(Timestamp now, uint32_t ts90khz); - absl::optional ExtrapolateLocalTime(uint32_t timestamp90khz); + absl::optional ExtrapolateLocalTime(uint32_t timestamp90khz) const; void Reset(Timestamp start); private: void CheckForWrapArounds(uint32_t ts90khz); bool DelayChangeDetection(double error); - double _w[2]; - double _pP[2][2]; - Timestamp _start; - Timestamp _prev; - uint32_t _firstTimestamp; - int32_t _wrapArounds; - int64_t _prevUnwrappedTimestamp; - int64_t _prevWrapTimestamp; - const double _lambda; - bool _firstAfterReset; - uint32_t _packetCount; - const uint32_t _startUpFilterDelayInPackets; - double _detectorAccumulatorPos; - double _detectorAccumulatorNeg; - const double _alarmThreshold; - const double _accDrift; - const double _accMaxError; - const double _pP11; + double w_[2]; + double p_[2][2]; + Timestamp start_; + Timestamp prev_; + absl::optional first_unwrapped_timestamp_; + TimestampUnwrapper unwrapper_; + absl::optional prev_unwrapped_timestamp_; + uint32_t packet_count_; + double detector_accumulator_pos_; + double detector_accumulator_neg_; }; } // namespace webrtc diff --git a/rtc_base/time/timestamp_extrapolator_unittest.cc b/rtc_base/time/timestamp_extrapolator_unittest.cc new file mode 100644 index 0000000000..b153d7ae15 --- /dev/null +++ b/rtc_base/time/timestamp_extrapolator_unittest.cc @@ -0,0 +1,208 @@ +/* + * Copyright (c) 2022 The WebRTC project authors. All Rights Reserved. + * + * Use of this source code is governed by a BSD-style license + * that can be found in the LICENSE file in the root of the source + * tree. An additional intellectual property rights grant can be found + * in the file PATENTS. All contributing project authors may + * be found in the AUTHORS file in the root of the source tree. + */ + +#include "rtc_base/time/timestamp_extrapolator.h" + +#include + +#include + +#include "absl/types/optional.h" +#include "api/units/frequency.h" +#include "api/units/time_delta.h" +#include "api/units/timestamp.h" +#include "system_wrappers/include/clock.h" +#include "test/gmock.h" +#include "test/gtest.h" + +namespace webrtc { + +using ::testing::Eq; +using ::testing::Optional; + +namespace { + +constexpr Frequency kRtpHz = Frequency::KiloHertz(90); +constexpr Frequency k25Fps = Frequency::Hertz(25); +constexpr TimeDelta k25FpsDelay = 1 / k25Fps; + +} // namespace + +TEST(TimestampExtrapolatorTest, ExtrapolationOccursAfter2Packets) { + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + // No packets so no timestamp. + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(90000), Eq(absl::nullopt)); + + uint32_t rtp = 90000; + clock.AdvanceTime(k25FpsDelay); + // First result is a bit confusing since it is based off the "start" time, + // which is arbitrary. + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000), + Optional(clock.CurrentTime() + TimeDelta::Seconds(1))); +} + +TEST(TimestampExtrapolatorTest, ResetsAfter10SecondPause) { + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + uint32_t rtp = 90000; + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + + rtp += 10 * kRtpHz.hertz(); + clock.AdvanceTime(TimeDelta::Seconds(10) + TimeDelta::Micros(1)); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); +} + +TEST(TimestampExtrapolatorTest, TimestampExtrapolatesMultipleRtpWrapArounds) { + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + uint32_t rtp = std::numeric_limits::max(); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + + // One overflow. Static cast to avoid undefined behaviour with +=. + rtp += static_cast(kRtpHz / k25Fps); + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + + // Assert that extrapolation works across the boundary as expected. + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000), + Optional(clock.CurrentTime() + TimeDelta::Seconds(1))); + // This is not quite 1s since the math always rounds up. + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp - 90000), + Optional(clock.CurrentTime() - TimeDelta::Millis(999))); + + // In order to avoid a wrap arounds reset, add a packet every 10s until we + // overflow twice. + constexpr TimeDelta kRtpOverflowDelay = + std::numeric_limits::max() / kRtpHz; + const Timestamp overflow_time = clock.CurrentTime() + kRtpOverflowDelay * 2; + + while (clock.CurrentTime() < overflow_time) { + clock.AdvanceTime(TimeDelta::Seconds(10)); + // Static-cast before += to avoid undefined behaviour of overflow. + rtp += static_cast(kRtpHz * TimeDelta::Seconds(10)); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + } +} + +TEST(TimestampExtrapolatorTest, Slow90KHzClock) { + // This simulates a slow camera, which produces frames at 24Hz instead of + // 25Hz. The extrapolator should be able to resolve this with enough data. + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + constexpr TimeDelta k24FpsDelay = 1 / Frequency::Hertz(24); + uint32_t rtp = 90000; + ts_extrapolator.Update(clock.CurrentTime(), rtp); + + // Slow camera will increment RTP at 25 FPS rate even though its producing at + // 24 FPS. After 25 frames the extrapolator should settle at this rate. + for (int i = 0; i < 25; ++i) { + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k24FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + } + + // The camera would normally produce 25 frames in 90K ticks, but is slow + // so takes 1s + k24FpsDelay for 90K ticks. + constexpr Frequency kSlowRtpHz = 90000 / (25 * k24FpsDelay); + // The extrapolator will be predicting that time at millisecond precision. + auto ts = ts_extrapolator.ExtrapolateLocalTime(rtp + kSlowRtpHz.hertz()); + ASSERT_TRUE(ts.has_value()); + EXPECT_EQ(ts->ms(), clock.TimeInMilliseconds() + 1000); +} + +TEST(TimestampExtrapolatorTest, Fast90KHzClock) { + // This simulates a fast camera, which produces frames at 26Hz instead of + // 25Hz. The extrapolator should be able to resolve this with enough data. + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + constexpr TimeDelta k26FpsDelay = 1 / Frequency::Hertz(26); + uint32_t rtp = 90000; + ts_extrapolator.Update(clock.CurrentTime(), rtp); + + // Fast camera will increment RTP at 25 FPS rate even though its producing at + // 26 FPS. After 25 frames the extrapolator should settle at this rate. + for (int i = 0; i < 25; ++i) { + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k26FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + } + + // The camera would normally produce 25 frames in 90K ticks, but is slow + // so takes 1s + k24FpsDelay for 90K ticks. + constexpr Frequency kSlowRtpHz = 90000 / (25 * k26FpsDelay); + // The extrapolator will be predicting that time at millisecond precision. + auto ts = ts_extrapolator.ExtrapolateLocalTime(rtp + kSlowRtpHz.hertz()); + ASSERT_TRUE(ts.has_value()); + EXPECT_EQ(ts->ms(), clock.TimeInMilliseconds() + 1000); +} + +TEST(TimestampExtrapolatorTest, TimestampJump) { + // This simulates a jump in RTP timestamp, which could occur if a camera was + // swapped for example. + SimulatedClock clock(Timestamp::Millis(1337)); + TimestampExtrapolator ts_extrapolator(clock.CurrentTime()); + + uint32_t rtp = 90000; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp), + Optional(clock.CurrentTime())); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000), + Optional(clock.CurrentTime() + TimeDelta::Seconds(1))); + + // Jump RTP. + uint32_t new_rtp = 1337 * 90000; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), new_rtp); + new_rtp += kRtpHz / k25Fps; + clock.AdvanceTime(k25FpsDelay); + ts_extrapolator.Update(clock.CurrentTime(), new_rtp); + EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(new_rtp), + Optional(clock.CurrentTime())); +} + +} // namespace webrtc