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 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 37 TimestampExtrapolator::~TimestampExtrapolator() 38 { 39 delete _rwLock; 40 } 41 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 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 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 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 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
