blob: b9243ca80c2a75d0b5df5f17134e2cb6c33a7379 [file] [log] [blame]
/*
* Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
*
* Use of this source code is governed by a BSD-style license
* that can be found in the LICENSE file in the root of the source
* tree. An additional intellectual property rights grant can be found
* in the file PATENTS. All contributing project authors may
* be found in the AUTHORS file in the root of the source tree.
*/
#include "webrtc/system_wrappers/include/timestamp_extrapolator.h"
#include <algorithm>
namespace webrtc {
TimestampExtrapolator::TimestampExtrapolator(int64_t start_ms)
: _rwLock(RWLockWrapper::CreateRWLock()),
_startMs(0),
_firstTimestamp(0),
_wrapArounds(0),
_prevUnwrappedTimestamp(-1),
_prevWrapTimestamp(-1),
_lambda(1),
_firstAfterReset(true),
_packetCount(0),
_startUpFilterDelayInPackets(2),
_detectorAccumulatorPos(0),
_detectorAccumulatorNeg(0),
_alarmThreshold(60e3),
_accDrift(6600), // in timestamp ticks, i.e. 15 ms
_accMaxError(7000),
_pP11(1e10) {
Reset(start_ms);
}
TimestampExtrapolator::~TimestampExtrapolator()
{
delete _rwLock;
}
void TimestampExtrapolator::Reset(int64_t start_ms)
{
WriteLockScoped wl(*_rwLock);
_startMs = start_ms;
_prevMs = _startMs;
_firstTimestamp = 0;
_w[0] = 90.0;
_w[1] = 0;
_pP[0][0] = 1;
_pP[1][1] = _pP11;
_pP[0][1] = _pP[1][0] = 0;
_firstAfterReset = true;
_prevUnwrappedTimestamp = -1;
_prevWrapTimestamp = -1;
_wrapArounds = 0;
_packetCount = 0;
_detectorAccumulatorPos = 0;
_detectorAccumulatorNeg = 0;
}
void
TimestampExtrapolator::Update(int64_t tMs, uint32_t ts90khz)
{
_rwLock->AcquireLockExclusive();
if (tMs - _prevMs > 10e3)
{
// Ten seconds without a complete frame.
// Reset the extrapolator
_rwLock->ReleaseLockExclusive();
Reset(tMs);
_rwLock->AcquireLockExclusive();
}
else
{
_prevMs = tMs;
}
// Remove offset to prevent badly scaled matrices
tMs -= _startMs;
CheckForWrapArounds(ts90khz);
int64_t unwrapped_ts90khz = static_cast<int64_t>(ts90khz) +
_wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
if (_firstAfterReset)
{
// Make an initial guess of the offset,
// should be almost correct since tMs - _startMs
// should about zero at this time.
_w[1] = -_w[0] * tMs;
_firstTimestamp = unwrapped_ts90khz;
_firstAfterReset = false;
}
double residual =
(static_cast<double>(unwrapped_ts90khz) - _firstTimestamp) -
static_cast<double>(tMs) * _w[0] - _w[1];
if (DelayChangeDetection(residual) &&
_packetCount >= _startUpFilterDelayInPackets)
{
// A sudden change of average network delay has been detected.
// Force the filter to adjust its offset parameter by changing
// the offset uncertainty. Don't do this during startup.
_pP[1][1] = _pP11;
}
if (_prevUnwrappedTimestamp >= 0 &&
unwrapped_ts90khz < _prevUnwrappedTimestamp)
{
// Drop reordered frames.
_rwLock->ReleaseLockExclusive();
return;
}
//T = [t(k) 1]';
//that = T'*w;
//K = P*T/(lambda + T'*P*T);
double K[2];
K[0] = _pP[0][0] * tMs + _pP[0][1];
K[1] = _pP[1][0] * tMs + _pP[1][1];
double TPT = _lambda + tMs * K[0] + K[1];
K[0] /= TPT;
K[1] /= TPT;
//w = w + K*(ts(k) - that);
_w[0] = _w[0] + K[0] * residual;
_w[1] = _w[1] + K[1] * residual;
//P = 1/lambda*(P - K*T'*P);
double p00 = 1 / _lambda *
(_pP[0][0] - (K[0] * tMs * _pP[0][0] + K[0] * _pP[1][0]));
double p01 = 1 / _lambda *
(_pP[0][1] - (K[0] * tMs * _pP[0][1] + K[0] * _pP[1][1]));
_pP[1][0] = 1 / _lambda *
(_pP[1][0] - (K[1] * tMs * _pP[0][0] + K[1] * _pP[1][0]));
_pP[1][1] = 1 / _lambda *
(_pP[1][1] - (K[1] * tMs * _pP[0][1] + K[1] * _pP[1][1]));
_pP[0][0] = p00;
_pP[0][1] = p01;
_prevUnwrappedTimestamp = unwrapped_ts90khz;
if (_packetCount < _startUpFilterDelayInPackets)
{
_packetCount++;
}
_rwLock->ReleaseLockExclusive();
}
int64_t
TimestampExtrapolator::ExtrapolateLocalTime(uint32_t timestamp90khz)
{
ReadLockScoped rl(*_rwLock);
int64_t localTimeMs = 0;
CheckForWrapArounds(timestamp90khz);
double unwrapped_ts90khz = static_cast<double>(timestamp90khz) +
_wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
if (_packetCount == 0)
{
localTimeMs = -1;
}
else if (_packetCount < _startUpFilterDelayInPackets)
{
localTimeMs = _prevMs + static_cast<int64_t>(
static_cast<double>(unwrapped_ts90khz - _prevUnwrappedTimestamp) /
90.0 + 0.5);
}
else
{
if (_w[0] < 1e-3)
{
localTimeMs = _startMs;
}
else
{
double timestampDiff = unwrapped_ts90khz -
static_cast<double>(_firstTimestamp);
localTimeMs = static_cast<int64_t>(
static_cast<double>(_startMs) + (timestampDiff - _w[1]) /
_w[0] + 0.5);
}
}
return localTimeMs;
}
// Investigates if the timestamp clock has overflowed since the last timestamp and
// keeps track of the number of wrap arounds since reset.
void
TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz)
{
if (_prevWrapTimestamp == -1)
{
_prevWrapTimestamp = ts90khz;
return;
}
if (ts90khz < _prevWrapTimestamp)
{
// This difference will probably be less than -2^31 if we have had a wrap around
// (e.g. timestamp = 1, _previousTimestamp = 2^32 - 1). Since it is casted to a Word32,
// it should be positive.
if (static_cast<int32_t>(ts90khz - _prevWrapTimestamp) > 0)
{
// Forward wrap around
_wrapArounds++;
}
}
// This difference will probably be less than -2^31 if we have had a backward wrap around.
// Since it is casted to a Word32, it should be positive.
else if (static_cast<int32_t>(_prevWrapTimestamp - ts90khz) > 0)
{
// Backward wrap around
_wrapArounds--;
}
_prevWrapTimestamp = ts90khz;
}
bool
TimestampExtrapolator::DelayChangeDetection(double error)
{
// CUSUM detection of sudden delay changes
error = (error > 0) ? std::min(error, _accMaxError) :
std::max(error, -_accMaxError);
_detectorAccumulatorPos =
std::max(_detectorAccumulatorPos + error - _accDrift, (double)0);
_detectorAccumulatorNeg =
std::min(_detectorAccumulatorNeg + error + _accDrift, (double)0);
if (_detectorAccumulatorPos > _alarmThreshold || _detectorAccumulatorNeg < -_alarmThreshold)
{
// Alarm
_detectorAccumulatorPos = _detectorAccumulatorNeg = 0;
return true;
}
return false;
}
}