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
15namespace webrtc {
16
17TimestampExtrapolator::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
37TimestampExtrapolator::~TimestampExtrapolator()
38{
39    delete _rwLock;
40}
41
42void 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
62void
63TimestampExtrapolator::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
148int64_t
149TimestampExtrapolator::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.
186void
187TimestampExtrapolator::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
215bool
216TimestampExtrapolator::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