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