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