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