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