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