92 lines
3.2 KiB
C++
92 lines
3.2 KiB
C++
/***************************************************************************
|
|
* Copyright (C) 2008 by Sindre Aamås *
|
|
* sinamas@users.sourceforge.net *
|
|
* *
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License version 2 as *
|
|
* published by the Free Software Foundation. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License version 2 for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU General Public License *
|
|
* version 2 along with this program; if not, write to the *
|
|
* Free Software Foundation, Inc., *
|
|
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA. *
|
|
***************************************************************************/
|
|
#include "rateest.h"
|
|
#include <cstdlib>
|
|
|
|
void RateEst::SumQueue::push(std::ptrdiff_t const samples, usec_t const usecs) {
|
|
q_.push_back(std::make_pair(samples, usecs));
|
|
samples_ += samples;
|
|
usecs_ += usecs;
|
|
}
|
|
|
|
void RateEst::SumQueue::pop() {
|
|
std::pair<std::ptrdiff_t, usec_t> const &f = q_.front();
|
|
samples_ -= f.first;
|
|
usecs_ -= f.second;
|
|
q_.pop_front();
|
|
}
|
|
|
|
static usec_t sampleUsecs(std::ptrdiff_t samples, long rate) {
|
|
return usec_t((samples * 1000000.0f) / (rate ? rate : 1) + 0.5f);
|
|
}
|
|
|
|
static long limit(long est, long const reference) {
|
|
if (est > reference + (reference >> 6))
|
|
est = reference + (reference >> 6);
|
|
else if (est < reference - (reference >> 6))
|
|
est = reference - (reference >> 6);
|
|
|
|
return est;
|
|
}
|
|
|
|
RateEst::RateEst(long const nominalSampleRate, std::size_t const maxValidFeedPeriodSamples)
|
|
: srate_(nominalSampleRate * est_scale)
|
|
, reference_(srate_)
|
|
, maxPeriod_(sampleUsecs(maxValidFeedPeriodSamples, nominalSampleRate))
|
|
, last_(0)
|
|
, t_(6000)
|
|
, s_(nominalSampleRate * 6)
|
|
, st_(s_ * t_)
|
|
, t2_(t_ * t_)
|
|
{
|
|
}
|
|
|
|
void RateEst::feed(std::ptrdiff_t samplesIn, usec_t const now) {
|
|
usec_t usecsIn = now - last_;
|
|
|
|
if (last_ && usecsIn < maxPeriod_) {
|
|
sumq_.push(samplesIn, usecsIn);
|
|
|
|
while ((usecsIn = sumq_.usecs()) > 100000) {
|
|
samplesIn = sumq_.samples();
|
|
sumq_.pop();
|
|
|
|
long const srateIn = long(samplesIn * (1000000.0f * est_scale) / usecsIn);
|
|
if (std::abs(srateIn - reference_) < reference_ >> 1) {
|
|
s_ += samplesIn - sumq_.samples() ;
|
|
t_ += ( usecsIn - sumq_.usecs() ) * 0.001;
|
|
st_ += s_ * t_;
|
|
t2_ += t_ * t_;
|
|
|
|
long est = long(st_ * (1000.0 * est_scale) / t2_ + 0.5);
|
|
srate_ = limit((srate_ * 31 + est + 16) >> 5, reference_);
|
|
|
|
if (t_ > 8000) {
|
|
s_ *= 3.0 / 4;
|
|
t_ *= 3.0 / 4;
|
|
st_ *= 9.0 / 16;
|
|
t2_ *= 9.0 / 16;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
last_ = now;
|
|
}
|