Line data Source code
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 :
17 0 : TimestampExtrapolator::TimestampExtrapolator(int64_t start_ms)
18 0 : : _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 0 : _pP11(1e10) {
34 0 : Reset(start_ms);
35 0 : }
36 :
37 0 : TimestampExtrapolator::~TimestampExtrapolator()
38 : {
39 0 : delete _rwLock;
40 0 : }
41 :
42 0 : void TimestampExtrapolator::Reset(int64_t start_ms)
43 : {
44 0 : WriteLockScoped wl(*_rwLock);
45 0 : _startMs = start_ms;
46 0 : _prevMs = _startMs;
47 0 : _firstTimestamp = 0;
48 0 : _w[0] = 90.0;
49 0 : _w[1] = 0;
50 0 : _pP[0][0] = 1;
51 0 : _pP[1][1] = _pP11;
52 0 : _pP[0][1] = _pP[1][0] = 0;
53 0 : _firstAfterReset = true;
54 0 : _prevUnwrappedTimestamp = -1;
55 0 : _prevWrapTimestamp = -1;
56 0 : _wrapArounds = 0;
57 0 : _packetCount = 0;
58 0 : _detectorAccumulatorPos = 0;
59 0 : _detectorAccumulatorNeg = 0;
60 0 : }
61 :
62 : void
63 0 : TimestampExtrapolator::Update(int64_t tMs, uint32_t ts90khz)
64 : {
65 :
66 0 : _rwLock->AcquireLockExclusive();
67 0 : if (tMs - _prevMs > 10e3)
68 : {
69 : // Ten seconds without a complete frame.
70 : // Reset the extrapolator
71 0 : _rwLock->ReleaseLockExclusive();
72 0 : Reset(tMs);
73 0 : _rwLock->AcquireLockExclusive();
74 : }
75 : else
76 : {
77 0 : _prevMs = tMs;
78 : }
79 :
80 : // Remove offset to prevent badly scaled matrices
81 0 : tMs -= _startMs;
82 :
83 0 : CheckForWrapArounds(ts90khz);
84 :
85 0 : int64_t unwrapped_ts90khz = static_cast<int64_t>(ts90khz) +
86 0 : _wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
87 :
88 0 : if (_prevUnwrappedTimestamp >= 0 &&
89 0 : unwrapped_ts90khz < _prevUnwrappedTimestamp)
90 : {
91 : // Drop reordered frames.
92 0 : _rwLock->ReleaseLockExclusive();
93 0 : return;
94 : }
95 :
96 0 : 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 0 : _w[1] = -_w[0] * tMs;
102 0 : _firstTimestamp = unwrapped_ts90khz;
103 0 : _firstAfterReset = false;
104 : }
105 :
106 : double residual =
107 0 : (static_cast<double>(unwrapped_ts90khz) - _firstTimestamp) -
108 0 : static_cast<double>(tMs) * _w[0] - _w[1];
109 0 : if (DelayChangeDetection(residual) &&
110 0 : _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 0 : _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 0 : K[0] = _pP[0][0] * tMs + _pP[0][1];
122 0 : K[1] = _pP[1][0] * tMs + _pP[1][1];
123 0 : double TPT = _lambda + tMs * K[0] + K[1];
124 0 : K[0] /= TPT;
125 0 : K[1] /= TPT;
126 : //w = w + K*(ts(k) - that);
127 0 : _w[0] = _w[0] + K[0] * residual;
128 0 : _w[1] = _w[1] + K[1] * residual;
129 : //P = 1/lambda*(P - K*T'*P);
130 0 : double p00 = 1 / _lambda *
131 0 : (_pP[0][0] - (K[0] * tMs * _pP[0][0] + K[0] * _pP[1][0]));
132 0 : double p01 = 1 / _lambda *
133 0 : (_pP[0][1] - (K[0] * tMs * _pP[0][1] + K[0] * _pP[1][1]));
134 0 : _pP[1][0] = 1 / _lambda *
135 0 : (_pP[1][0] - (K[1] * tMs * _pP[0][0] + K[1] * _pP[1][0]));
136 0 : _pP[1][1] = 1 / _lambda *
137 0 : (_pP[1][1] - (K[1] * tMs * _pP[0][1] + K[1] * _pP[1][1]));
138 0 : _pP[0][0] = p00;
139 0 : _pP[0][1] = p01;
140 0 : _prevUnwrappedTimestamp = unwrapped_ts90khz;
141 0 : if (_packetCount < _startUpFilterDelayInPackets)
142 : {
143 0 : _packetCount++;
144 : }
145 0 : _rwLock->ReleaseLockExclusive();
146 : }
147 :
148 : int64_t
149 0 : TimestampExtrapolator::ExtrapolateLocalTime(uint32_t timestamp90khz)
150 : {
151 0 : ReadLockScoped rl(*_rwLock);
152 0 : int64_t localTimeMs = 0;
153 0 : CheckForWrapArounds(timestamp90khz);
154 0 : double unwrapped_ts90khz = static_cast<double>(timestamp90khz) +
155 0 : _wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
156 0 : if (_packetCount == 0)
157 : {
158 0 : localTimeMs = -1;
159 : }
160 0 : else if (_packetCount < _startUpFilterDelayInPackets)
161 : {
162 0 : localTimeMs = _prevMs + static_cast<int64_t>(
163 0 : static_cast<double>(unwrapped_ts90khz - _prevUnwrappedTimestamp) /
164 0 : 90.0 + 0.5);
165 : }
166 : else
167 : {
168 0 : if (_w[0] < 1e-3)
169 : {
170 0 : localTimeMs = _startMs;
171 : }
172 : else
173 : {
174 : double timestampDiff = unwrapped_ts90khz -
175 0 : static_cast<double>(_firstTimestamp);
176 0 : localTimeMs = static_cast<int64_t>(
177 0 : static_cast<double>(_startMs) + (timestampDiff - _w[1]) /
178 0 : _w[0] + 0.5);
179 : }
180 : }
181 0 : 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
187 0 : TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz)
188 : {
189 0 : if (_prevWrapTimestamp == -1)
190 : {
191 0 : _prevWrapTimestamp = ts90khz;
192 0 : return;
193 : }
194 0 : 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 0 : if (static_cast<int32_t>(ts90khz - _prevWrapTimestamp) > 0)
200 : {
201 : // Forward wrap around
202 0 : _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 0 : else if (static_cast<int32_t>(_prevWrapTimestamp - ts90khz) > 0)
208 : {
209 : // Backward wrap around
210 0 : _wrapArounds--;
211 : }
212 0 : _prevWrapTimestamp = ts90khz;
213 : }
214 :
215 : bool
216 0 : TimestampExtrapolator::DelayChangeDetection(double error)
217 : {
218 : // CUSUM detection of sudden delay changes
219 0 : error = (error > 0) ? std::min(error, _accMaxError) :
220 0 : std::max(error, -_accMaxError);
221 0 : _detectorAccumulatorPos =
222 0 : std::max(_detectorAccumulatorPos + error - _accDrift, (double)0);
223 0 : _detectorAccumulatorNeg =
224 0 : std::min(_detectorAccumulatorNeg + error + _accDrift, (double)0);
225 0 : if (_detectorAccumulatorPos > _alarmThreshold || _detectorAccumulatorNeg < -_alarmThreshold)
226 : {
227 : // Alarm
228 0 : _detectorAccumulatorPos = _detectorAccumulatorNeg = 0;
229 0 : return true;
230 : }
231 0 : return false;
232 : }
233 :
234 : }
|
