269 lines
6.5 KiB
C++
269 lines
6.5 KiB
C++
// -------------------------------------------------------------------------
|
|
//
|
|
// Copyright (C) 2004-2013 Fons Adriaensen <fons@linuxaudio.org>
|
|
//
|
|
// This program is free software; you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation; either version 2 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// 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 for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
//
|
|
// -------------------------------------------------------------------------
|
|
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "analyser.h"
|
|
|
|
|
|
Trace::Trace (int size) : _valid (false)
|
|
{
|
|
_data = new float [size];
|
|
}
|
|
|
|
|
|
Trace::~Trace (void)
|
|
{
|
|
delete[] _data;
|
|
}
|
|
|
|
|
|
Analyser::Analyser (int ipsize, int fftmax, float fsamp) :
|
|
_ipsize (ipsize),
|
|
_icount (0),
|
|
_fftmax (fftmax),
|
|
_fftlen (0),
|
|
_fftplan (0),
|
|
_fsamp (fsamp),
|
|
_wfact (0.0f),
|
|
_speed (1.0f)
|
|
{
|
|
_ipdata = new float [ipsize];
|
|
_warped = (float *) fftwf_malloc ((fftmax + 1) * sizeof (float));
|
|
_trdata = (fftwf_complex *) fftwf_malloc ((fftmax / 2 + 9) * sizeof (fftwf_complex));
|
|
_power = new Trace (fftmax + 1);
|
|
_peakp = new Trace (fftmax + 1);
|
|
}
|
|
|
|
|
|
Analyser::~Analyser (void)
|
|
{
|
|
#ifdef WITH_FFTW_LOCK
|
|
pthread_mutex_lock(&fftw_planner_lock);
|
|
#endif
|
|
if (_fftplan) fftwf_destroy_plan (_fftplan);
|
|
#ifdef WITH_FFTW_LOCK
|
|
if (instance_count > 0) {
|
|
--instance_count;
|
|
}
|
|
# ifdef WITH_STATIC_FFTW_CLEANUP
|
|
if (instance_count == 0) {
|
|
fftwf_cleanup ();
|
|
}
|
|
# endif
|
|
pthread_mutex_unlock(&fftw_planner_lock);
|
|
#endif
|
|
delete _power;
|
|
delete _peakp;
|
|
fftwf_free (_trdata);
|
|
fftwf_free (_warped);
|
|
delete[] _ipdata;
|
|
}
|
|
|
|
|
|
void Analyser::set_fftlen (int fftlen)
|
|
{
|
|
if (fftlen > _fftmax) fftlen = _fftmax;
|
|
if (_fftlen != fftlen)
|
|
{
|
|
#ifdef WITH_FFTW_LOCK
|
|
pthread_mutex_lock(&fftw_planner_lock);
|
|
#endif
|
|
if (_fftplan) {
|
|
fftwf_destroy_plan (_fftplan);
|
|
}
|
|
#ifdef WITH_FFTW_LOCK
|
|
else {
|
|
++instance_count;
|
|
}
|
|
#endif
|
|
_fftlen = fftlen;
|
|
_fftplan = fftwf_plan_dft_r2c_1d (_fftlen, _warped, _trdata + 4, FFTW_ESTIMATE);
|
|
#ifdef WITH_FFTW_LOCK
|
|
pthread_mutex_unlock(&fftw_planner_lock);
|
|
#endif
|
|
set_wfact (_wfact);
|
|
set_speed (_speed);
|
|
clr_peak ();
|
|
}
|
|
}
|
|
|
|
|
|
void Analyser::set_wfact (float wfact)
|
|
{
|
|
_wfact = wfact;
|
|
_pmax = 1e-20f;
|
|
memset (_warped, 0, (_fftlen + 1) * sizeof (float));
|
|
_power->_valid = false;
|
|
_peakp->_valid = false;
|
|
memset (_power->_data, 0, (_fftlen + 1) * sizeof (float));
|
|
memset (_peakp->_data, 0, (_fftlen + 1) * sizeof (float));
|
|
}
|
|
|
|
|
|
void Analyser::set_speed (float speed)
|
|
{
|
|
_speed = speed;
|
|
}
|
|
|
|
|
|
void Analyser::clr_peak (void)
|
|
{
|
|
_peakp->_valid = false;
|
|
memset (_peakp->_data, 0, (_fftlen + 1) * sizeof (float));
|
|
}
|
|
|
|
|
|
float Analyser::conv0 (fftwf_complex *v)
|
|
{
|
|
float x, y;
|
|
x = v [0][0]
|
|
- 0.677014f * (v [-1][0] + v [1][0])
|
|
+ 0.195602f * (v [-2][0] + v [2][0])
|
|
- 0.019420f * (v [-3][0] + v [3][0])
|
|
+ 0.000741f * (v [-4][0] + v [4][0]);
|
|
y = v [0][1]
|
|
- 0.677014f * (v [-1][1] + v [1][1])
|
|
+ 0.195602f * (v [-2][1] + v [2][1])
|
|
- 0.019420f * (v [-3][1] + v [3][1])
|
|
+ 0.000741f * (v [-4][1] + v [4][1]);
|
|
return x * x + y * y;
|
|
}
|
|
|
|
|
|
float Analyser::conv1 (fftwf_complex *v)
|
|
{
|
|
float x, y;
|
|
x = 0.908040f * (v [ 0][0] - v [1][0])
|
|
- 0.409037f * (v [-1][0] - v [2][0])
|
|
+ 0.071556f * (v [-2][0] - v [3][0])
|
|
- 0.004085f * (v [-3][0] - v [4][0]);
|
|
y = 0.908040f * (v [ 0][1] - v [1][1])
|
|
- 0.409037f * (v [-1][1] - v [2][1])
|
|
+ 0.071556f * (v [-2][1] - v [3][1])
|
|
- 0.004085f * (v [-3][1] - v [4][1]);
|
|
return x * x + y * y;
|
|
}
|
|
|
|
|
|
void Analyser::process (int iplen, bool holdp)
|
|
{
|
|
int i, j, k, l;
|
|
float a, b, c, d, m, p, s, w, z;
|
|
float *p1, *p2;
|
|
|
|
w = -_wfact;
|
|
l = _fftlen / 2;
|
|
|
|
for (k = 0; k < iplen; k += l)
|
|
{
|
|
p1 = _ipdata + _icount;
|
|
_icount += l;
|
|
if (_icount == _ipsize) _icount = 0;
|
|
|
|
for (j = 0; j < l; j += 4)
|
|
{
|
|
a = _warped [0];
|
|
b = *p1++ + 1e-20f;
|
|
c = *p1++ - 1e-20f;
|
|
d = *p1++ + 1e-20f;
|
|
_warped [0] = z = *p1++ - 1e-20f;
|
|
for (i = 0; i < _fftlen; i += 4)
|
|
{
|
|
s = _warped [i + 1];
|
|
a += w * (b - s);
|
|
b += w * (c - a);
|
|
c += w * (d - b);
|
|
_warped [i + 1] = z = d + w * (z - c);
|
|
d = s;
|
|
s = _warped [i + 2];
|
|
d += w * (a - s);
|
|
a += w * (b - d);
|
|
b += w * (c - a);
|
|
_warped [i + 2] = z = c + w * (z - b);
|
|
c = s;
|
|
s = _warped [i + 3];
|
|
c += w * (d - s);
|
|
d += w * (a - c);
|
|
a += w * (b - d);
|
|
_warped [i + 3] = z = b + w * (z - a);
|
|
b = s;
|
|
s = _warped [i + 4];
|
|
b += w * (c - s);
|
|
c += w * (d - b);
|
|
d += w * (a - c);
|
|
_warped [i + 4] = z = a + w * (z - d);
|
|
a = s;
|
|
}
|
|
}
|
|
|
|
fftwf_execute (_fftplan);
|
|
for (i = 1; i <= 4; i++)
|
|
{
|
|
_trdata [4 - i][0] = _trdata [4 + i][0];
|
|
_trdata [4 - i][1] = -_trdata [4 + i][1];
|
|
_trdata [4 + l + i][0] = _trdata [4 + l - i][0];
|
|
_trdata [4 + l + i][1] = -_trdata [4 + l - i][1];
|
|
}
|
|
|
|
a = 1.0f - powf (0.1f, l / (_fsamp * _speed));
|
|
b = 4.0f / ((float)_fftlen * (float)_fftlen);
|
|
s = 0;
|
|
m = 0;
|
|
p1 = _power->_data;
|
|
for (i = 0; i < l; i++)
|
|
{
|
|
p = b * conv0 (_trdata + 4 + i) + 1e-20f;
|
|
if (m < p) m = p;
|
|
s += p;
|
|
*p1 += a * (p - *p1);
|
|
p1++;
|
|
p = b * conv1 (_trdata + 4 + i) + 1e-20f;
|
|
if (m < p) m = p;
|
|
s += p;
|
|
*p1 += a * (p - *p1);
|
|
p1++;
|
|
}
|
|
p = b * conv0 (_trdata + 4 + i) + 1e-20f;
|
|
s += p;
|
|
*p1 += a * (p - *p1);
|
|
_power->_valid = true;
|
|
|
|
if (_pmax < m) _pmax = m;
|
|
else _pmax *= 0.95f;
|
|
_ptot = s;
|
|
|
|
if (holdp)
|
|
{
|
|
p1 = _power->_data;
|
|
p2 = _peakp->_data;
|
|
for (i = 0; i <= 2 * l; i++)
|
|
{
|
|
if (p2 [i] < p1 [i]) p2 [i] = p1 [i];
|
|
}
|
|
_peakp->_valid = true;
|
|
}
|
|
}
|
|
}
|
|
|