sdrangel/wdsp/anr.cpp

214 wiersze
4.9 KiB
C++

/* anr.c
This file is part of a program that implements a Software-Defined Radio.
Copyright (C) 2012, 2013 Warren Pratt, NR0V
Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
The author can be reached by email at
warren@wpratt.com
*/
#include "comm.hpp"
#include "anr.hpp"
#include "amd.hpp"
#include "snba.hpp"
#include "emnr.hpp"
#include "anf.hpp"
#include "bandpass.hpp"
namespace WDSP {
ANR::ANR(
int _run,
int _position,
int _buff_size,
float *_in_buff,
float *_out_buff,
int _dline_size,
int _n_taps,
int _delay,
double _two_mu,
double _gamma,
double _lidx,
double _lidx_min,
double _lidx_max,
double _ngamma,
double _den_mult,
double _lincr,
double _ldecr
) :
run(_run),
position(_position),
buff_size(_buff_size),
in_buff(_in_buff),
out_buff(_out_buff),
dline_size(_dline_size),
mask(_dline_size - 1),
n_taps(_n_taps),
delay(_delay),
two_mu(_two_mu),
gamma(_gamma),
in_idx(0),
lidx(_lidx),
lidx_min(_lidx_min),
lidx_max(_lidx_max),
ngamma(_ngamma),
den_mult(_den_mult),
lincr(_lincr),
ldecr(_ldecr)
{
std::fill(d.begin(), d.end(), 0);
std::fill(w.begin(), w.end(), 0);
}
void ANR::execute(int _position)
{
int idx;
double c0;
double c1;
double y;
double error;
double sigma;
double inv_sigp;
double nel;
double nev;
if (run && (position == _position))
{
for (int i = 0; i < buff_size; i++)
{
d[in_idx] = in_buff[2 * i + 0];
y = 0;
sigma = 0;
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
y += w[j] * d[idx];
sigma += d[idx] * d[idx];
}
inv_sigp = 1.0 / (sigma + 1e-10);
error = d[in_idx] - y;
out_buff[2 * i + 0] = (float) y;
out_buff[2 * i + 1] = 0.0;
if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0)
nel = -nel;
if ((nev = d[in_idx] - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0)
nev = -nev;
if (nev < nel)
{
if ((lidx += lincr) > lidx_max)
lidx = lidx_max;
}
else
{
if ((lidx -= ldecr) < lidx_min)
lidx = lidx_min;
}
ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult;
c0 = 1.0 - two_mu * ngamma;
c1 = two_mu * error * inv_sigp;
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
w[j] = c0 * w[j] + c1 * d[idx];
}
in_idx = (in_idx + mask) & mask;
}
}
else if (in_buff != out_buff)
{
std::copy(in_buff, in_buff + buff_size * 2, out_buff);
}
}
void ANR::flush()
{
std::fill(d.begin(), d.end(), 0);
std::fill(w.begin(), w.end(), 0);
in_idx = 0;
}
void ANR::setBuffers(float* _in, float* _out)
{
in_buff = _in;
out_buff = _out;
}
void ANR::setSamplerate(int)
{
flush();
}
void ANR::setSize(int _size)
{
buff_size = _size;
flush();
}
/********************************************************************************************************
* *
* Public Properties *
* *
********************************************************************************************************/
void ANR::setVals(int _taps, int _delay, double _gain, double _leakage)
{
n_taps = _taps;
delay = _delay;
two_mu = _gain;
gamma = _leakage;
flush();
}
void ANR::setTaps(int _taps)
{
n_taps = _taps;
flush();
}
void ANR::setDelay(int _delay)
{
delay = _delay;
flush();
}
void ANR::setGain(double _gain)
{
two_mu = _gain;
flush();
}
void ANR::setLeakage(double _leakage)
{
gamma = _leakage;
flush();
}
} // namespace WDSP