src/third_party/webrtc/system_wrappers/source/timestamp_extrapolator.cc

   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
  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 }