1 /*
2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
3 *
4 * Use of this source code is governed by a BSD-style license
5 * that can be found in the LICENSE file in the root of the source
6 * tree. An additional intellectual property rights grant can be found
7 * in the file PATENTS. All contributing project authors may
8 * be found in the AUTHORS file in the root of the source tree.
9 */
10
11 #include "rtc_base/time/timestamp_extrapolator.h"
12
13 #include <algorithm>
14
15 namespace webrtc {
16
TimestampExtrapolator(int64_t start_ms)17 TimestampExtrapolator::TimestampExtrapolator(int64_t start_ms)
18 : _startMs(0),
19 _firstTimestamp(0),
20 _wrapArounds(0),
21 _prevUnwrappedTimestamp(-1),
22 _prevWrapTimestamp(-1),
23 _lambda(1),
24 _firstAfterReset(true),
25 _packetCount(0),
26 _startUpFilterDelayInPackets(2),
27 _detectorAccumulatorPos(0),
28 _detectorAccumulatorNeg(0),
29 _alarmThreshold(60e3),
30 _accDrift(6600), // in timestamp ticks, i.e. 15 ms
31 _accMaxError(7000),
32 _pP11(1e10) {
33 Reset(start_ms);
34 }
35
Reset(int64_t start_ms)36 void TimestampExtrapolator::Reset(int64_t start_ms) {
37 _startMs = start_ms;
38 _prevMs = _startMs;
39 _firstTimestamp = 0;
40 _w[0] = 90.0;
41 _w[1] = 0;
42 _pP[0][0] = 1;
43 _pP[1][1] = _pP11;
44 _pP[0][1] = _pP[1][0] = 0;
45 _firstAfterReset = true;
46 _prevUnwrappedTimestamp = -1;
47 _prevWrapTimestamp = -1;
48 _wrapArounds = 0;
49 _packetCount = 0;
50 _detectorAccumulatorPos = 0;
51 _detectorAccumulatorNeg = 0;
52 }
53
Update(int64_t tMs,uint32_t ts90khz)54 void TimestampExtrapolator::Update(int64_t tMs, uint32_t ts90khz) {
55 if (tMs - _prevMs > 10e3) {
56 // Ten seconds without a complete frame.
57 // Reset the extrapolator
58 Reset(tMs);
59 } else {
60 _prevMs = tMs;
61 }
62
63 // Remove offset to prevent badly scaled matrices
64 tMs -= _startMs;
65
66 CheckForWrapArounds(ts90khz);
67
68 int64_t unwrapped_ts90khz =
69 static_cast<int64_t>(ts90khz) +
70 _wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
71
72 if (_firstAfterReset) {
73 // Make an initial guess of the offset,
74 // should be almost correct since tMs - _startMs
75 // should about zero at this time.
76 _w[1] = -_w[0] * tMs;
77 _firstTimestamp = unwrapped_ts90khz;
78 _firstAfterReset = false;
79 }
80
81 double residual = (static_cast<double>(unwrapped_ts90khz) - _firstTimestamp) -
82 static_cast<double>(tMs) * _w[0] - _w[1];
83 if (DelayChangeDetection(residual) &&
84 _packetCount >= _startUpFilterDelayInPackets) {
85 // A sudden change of average network delay has been detected.
86 // Force the filter to adjust its offset parameter by changing
87 // the offset uncertainty. Don't do this during startup.
88 _pP[1][1] = _pP11;
89 }
90
91 if (_prevUnwrappedTimestamp >= 0 &&
92 unwrapped_ts90khz < _prevUnwrappedTimestamp) {
93 // Drop reordered frames.
94 return;
95 }
96
97 // T = [t(k) 1]';
98 // that = T'*w;
99 // K = P*T/(lambda + T'*P*T);
100 double K[2];
101 K[0] = _pP[0][0] * tMs + _pP[0][1];
102 K[1] = _pP[1][0] * tMs + _pP[1][1];
103 double TPT = _lambda + tMs * K[0] + K[1];
104 K[0] /= TPT;
105 K[1] /= TPT;
106 // w = w + K*(ts(k) - that);
107 _w[0] = _w[0] + K[0] * residual;
108 _w[1] = _w[1] + K[1] * residual;
109 // P = 1/lambda*(P - K*T'*P);
110 double p00 =
111 1 / _lambda * (_pP[0][0] - (K[0] * tMs * _pP[0][0] + K[0] * _pP[1][0]));
112 double p01 =
113 1 / _lambda * (_pP[0][1] - (K[0] * tMs * _pP[0][1] + K[0] * _pP[1][1]));
114 _pP[1][0] =
115 1 / _lambda * (_pP[1][0] - (K[1] * tMs * _pP[0][0] + K[1] * _pP[1][0]));
116 _pP[1][1] =
117 1 / _lambda * (_pP[1][1] - (K[1] * tMs * _pP[0][1] + K[1] * _pP[1][1]));
118 _pP[0][0] = p00;
119 _pP[0][1] = p01;
120 _prevUnwrappedTimestamp = unwrapped_ts90khz;
121 if (_packetCount < _startUpFilterDelayInPackets) {
122 _packetCount++;
123 }
124 }
125
ExtrapolateLocalTime(uint32_t timestamp90khz)126 int64_t TimestampExtrapolator::ExtrapolateLocalTime(uint32_t timestamp90khz) {
127 int64_t localTimeMs = 0;
128 CheckForWrapArounds(timestamp90khz);
129 double unwrapped_ts90khz =
130 static_cast<double>(timestamp90khz) +
131 _wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
132 if (_packetCount == 0) {
133 localTimeMs = -1;
134 } else if (_packetCount < _startUpFilterDelayInPackets) {
135 localTimeMs =
136 _prevMs +
137 static_cast<int64_t>(
138 static_cast<double>(unwrapped_ts90khz - _prevUnwrappedTimestamp) /
139 90.0 +
140 0.5);
141 } else {
142 if (_w[0] < 1e-3) {
143 localTimeMs = _startMs;
144 } else {
145 double timestampDiff =
146 unwrapped_ts90khz - static_cast<double>(_firstTimestamp);
147 localTimeMs = static_cast<int64_t>(static_cast<double>(_startMs) +
148 (timestampDiff - _w[1]) / _w[0] + 0.5);
149 }
150 }
151 return localTimeMs;
152 }
153
154 // Investigates if the timestamp clock has overflowed since the last timestamp
155 // and keeps track of the number of wrap arounds since reset.
CheckForWrapArounds(uint32_t ts90khz)156 void TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz) {
157 if (_prevWrapTimestamp == -1) {
158 _prevWrapTimestamp = ts90khz;
159 return;
160 }
161 if (ts90khz < _prevWrapTimestamp) {
162 // This difference will probably be less than -2^31 if we have had a wrap
163 // around (e.g. timestamp = 1, _previousTimestamp = 2^32 - 1). Since it is
164 // casted to a Word32, it should be positive.
165 if (static_cast<int32_t>(ts90khz - _prevWrapTimestamp) > 0) {
166 // Forward wrap around
167 _wrapArounds++;
168 }
169 } else {
170 // This difference will probably be less than -2^31 if we have had a
171 // backward wrap around. Since it is casted to a Word32, it should be
172 // positive.
173 if (static_cast<int32_t>(_prevWrapTimestamp - ts90khz) > 0) {
174 // Backward wrap around
175 _wrapArounds--;
176 }
177 }
178 _prevWrapTimestamp = ts90khz;
179 }
180
DelayChangeDetection(double error)181 bool TimestampExtrapolator::DelayChangeDetection(double error) {
182 // CUSUM detection of sudden delay changes
183 error = (error > 0) ? std::min(error, _accMaxError)
184 : std::max(error, -_accMaxError);
185 _detectorAccumulatorPos =
186 std::max(_detectorAccumulatorPos + error - _accDrift, double{0});
187 _detectorAccumulatorNeg =
188 std::min(_detectorAccumulatorNeg + error + _accDrift, double{0});
189 if (_detectorAccumulatorPos > _alarmThreshold ||
190 _detectorAccumulatorNeg < -_alarmThreshold) {
191 // Alarm
192 _detectorAccumulatorPos = _detectorAccumulatorNeg = 0;
193 return true;
194 }
195 return false;
196 }
197
198 } // namespace webrtc
199