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/interface/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 _P11(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 _P[0][0] = 1; 51 _P[1][1] = _P11; 52 _P[0][1] = _P[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 _P[1][1] = _P11; 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] = _P[0][0] * tMs + _P[0][1]; 122 K[1] = _P[1][0] * tMs + _P[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 * (_P[0][0] - (K[0] * tMs * _P[0][0] + K[0] * _P[1][0])); 131 double p01 = 1 / _lambda * (_P[0][1] - (K[0] * tMs * _P[0][1] + K[0] * _P[1][1])); 132 _P[1][0] = 1 / _lambda * (_P[1][0] - (K[1] * tMs * _P[0][0] + K[1] * _P[1][0])); 133 _P[1][1] = 1 / _lambda * (_P[1][1] - (K[1] * tMs * _P[0][1] + K[1] * _P[1][1])); 134 _P[0][0] = p00; 135 _P[0][1] = p01; 136 _prevUnwrappedTimestamp = unwrapped_ts90khz; 137 if (_packetCount < _startUpFilterDelayInPackets) 138 { 139 _packetCount++; 140 } 141 _rwLock->ReleaseLockExclusive(); 142} 143 144int64_t 145TimestampExtrapolator::ExtrapolateLocalTime(uint32_t timestamp90khz) 146{ 147 ReadLockScoped rl(*_rwLock); 148 int64_t localTimeMs = 0; 149 CheckForWrapArounds(timestamp90khz); 150 double unwrapped_ts90khz = static_cast<double>(timestamp90khz) + 151 _wrapArounds * ((static_cast<int64_t>(1) << 32) - 1); 152 if (_packetCount == 0) 153 { 154 localTimeMs = -1; 155 } 156 else if (_packetCount < _startUpFilterDelayInPackets) 157 { 158 localTimeMs = _prevMs + static_cast<int64_t>( 159 static_cast<double>(unwrapped_ts90khz - _prevUnwrappedTimestamp) / 160 90.0 + 0.5); 161 } 162 else 163 { 164 if (_w[0] < 1e-3) 165 { 166 localTimeMs = _startMs; 167 } 168 else 169 { 170 double timestampDiff = unwrapped_ts90khz - 171 static_cast<double>(_firstTimestamp); 172 localTimeMs = static_cast<int64_t>( 173 static_cast<double>(_startMs) + (timestampDiff - _w[1]) / 174 _w[0] + 0.5); 175 } 176 } 177 return localTimeMs; 178} 179 180// Investigates if the timestamp clock has overflowed since the last timestamp and 181// keeps track of the number of wrap arounds since reset. 182void 183TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz) 184{ 185 if (_prevWrapTimestamp == -1) 186 { 187 _prevWrapTimestamp = ts90khz; 188 return; 189 } 190 if (ts90khz < _prevWrapTimestamp) 191 { 192 // This difference will probably be less than -2^31 if we have had a wrap around 193 // (e.g. timestamp = 1, _previousTimestamp = 2^32 - 1). Since it is casted to a Word32, 194 // it should be positive. 195 if (static_cast<int32_t>(ts90khz - _prevWrapTimestamp) > 0) 196 { 197 // Forward wrap around 198 _wrapArounds++; 199 } 200 } 201 // This difference will probably be less than -2^31 if we have had a backward wrap around. 202 // Since it is casted to a Word32, it should be positive. 203 else if (static_cast<int32_t>(_prevWrapTimestamp - ts90khz) > 0) 204 { 205 // Backward wrap around 206 _wrapArounds--; 207 } 208 _prevWrapTimestamp = ts90khz; 209} 210 211bool 212TimestampExtrapolator::DelayChangeDetection(double error) 213{ 214 // CUSUM detection of sudden delay changes 215 error = (error > 0) ? std::min(error, _accMaxError) : 216 std::max(error, -_accMaxError); 217 _detectorAccumulatorPos = 218 std::max(_detectorAccumulatorPos + error - _accDrift, (double)0); 219 _detectorAccumulatorNeg = 220 std::min(_detectorAccumulatorNeg + error + _accDrift, (double)0); 221 if (_detectorAccumulatorPos > _alarmThreshold || _detectorAccumulatorNeg < -_alarmThreshold) 222 { 223 // Alarm 224 _detectorAccumulatorPos = _detectorAccumulatorNeg = 0; 225 return true; 226 } 227 return false; 228} 229 230} 231