kopia lustrzana https://github.com/f4exb/sdrangel
495 wiersze
12 KiB
C++
495 wiersze
12 KiB
C++
/* cfcomp.c
|
|
|
|
This file is part of a program that implements a Software-Defined Radio.
|
|
|
|
Copyright (C) 2017, 2021 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 "cfcomp.hpp"
|
|
#include "meterlog10.hpp"
|
|
#include "TXA.hpp"
|
|
|
|
namespace WDSP {
|
|
|
|
void CFCOMP::calc_cfcwindow()
|
|
{
|
|
int i;
|
|
double arg0;
|
|
double arg1;
|
|
double cgsum;
|
|
double igsum;
|
|
double coherent_gain;
|
|
double inherent_power_gain;
|
|
double wmult;
|
|
switch (wintype)
|
|
{
|
|
case 0:
|
|
arg0 = 2.0 * PI / (float)fsize;
|
|
cgsum = 0.0;
|
|
igsum = 0.0;
|
|
for (i = 0; i < fsize; i++)
|
|
{
|
|
window[i] = sqrt (0.54 - 0.46 * cos((float)i * arg0));
|
|
cgsum += window[i];
|
|
igsum += window[i] * window[i];
|
|
}
|
|
coherent_gain = cgsum / (float)fsize;
|
|
inherent_power_gain = igsum / (float)fsize;
|
|
wmult = 1.0 / sqrt (inherent_power_gain);
|
|
for (i = 0; i < fsize; i++)
|
|
window[i] *= wmult;
|
|
winfudge = sqrt (1.0 / coherent_gain);
|
|
break;
|
|
case 1:
|
|
arg0 = 2.0 * PI / (float)fsize;
|
|
cgsum = 0.0;
|
|
igsum = 0.0;
|
|
for (i = 0; i < fsize; i++)
|
|
{
|
|
arg1 = cos(arg0 * (float)i);
|
|
window[i] = sqrt (+0.21747
|
|
+ arg1 * (-0.45325
|
|
+ arg1 * (+0.28256
|
|
+ arg1 * (-0.04672))));
|
|
cgsum += window[i];
|
|
igsum += window[i] * window[i];
|
|
}
|
|
coherent_gain = cgsum / (float)fsize;
|
|
inherent_power_gain = igsum / (float)fsize;
|
|
wmult = 1.0 / sqrt (inherent_power_gain);
|
|
for (i = 0; i < fsize; i++)
|
|
window[i] *= wmult;
|
|
winfudge = sqrt (1.0 / coherent_gain);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
int CFCOMP::fCOMPcompare (const void *a, const void *b)
|
|
{
|
|
if (*(double*)a < *(double*)b)
|
|
return -1;
|
|
else if (*(double*)a == *(double*)b)
|
|
return 0;
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
void CFCOMP::calc_comp()
|
|
{
|
|
int i;
|
|
int j;
|
|
double f;
|
|
double frac;
|
|
double fincr;
|
|
double fmax;
|
|
double* sary;
|
|
precomplin = pow (10.0, 0.05 * precomp);
|
|
prepeqlin = pow (10.0, 0.05 * prepeq);
|
|
fmax = 0.5 * rate;
|
|
for (i = 0; i < nfreqs; i++)
|
|
{
|
|
F[i] = std::max (F[i], 0.0);
|
|
F[i] = std::min (F[i], fmax);
|
|
G[i] = std::max (G[i], 0.0);
|
|
}
|
|
sary = new double[3 * nfreqs];
|
|
for (i = 0; i < nfreqs; i++)
|
|
{
|
|
sary[3 * i + 0] = F[i];
|
|
sary[3 * i + 1] = G[i];
|
|
sary[3 * i + 2] = E[i];
|
|
}
|
|
qsort (sary, nfreqs, 3 * sizeof (float), fCOMPcompare);
|
|
for (i = 0; i < nfreqs; i++)
|
|
{
|
|
F[i] = sary[3 * i + 0];
|
|
G[i] = sary[3 * i + 1];
|
|
E[i] = sary[3 * i + 2];
|
|
}
|
|
fp[0] = 0.0;
|
|
fp[nfreqs + 1] = fmax;
|
|
gp[0] = G[0];
|
|
gp[nfreqs + 1] = G[nfreqs - 1];
|
|
ep[0] = E[0]; // cutoff?
|
|
ep[nfreqs + 1] = E[nfreqs - 1]; // cutoff?
|
|
for (i = 0, j = 1; i < nfreqs; i++, j++)
|
|
{
|
|
fp[j] = F[i];
|
|
gp[j] = G[i];
|
|
ep[j] = E[i];
|
|
}
|
|
fincr = rate / (float)fsize;
|
|
j = 0;
|
|
|
|
for (i = 0; i < msize; i++)
|
|
{
|
|
f = fincr * (float)i;
|
|
while (f >= fp[j + 1] && j < nfreqs) j++;
|
|
frac = (f - fp[j]) / (fp[j + 1] - fp[j]);
|
|
comp[i] = pow (10.0, 0.05 * (frac * gp[j + 1] + (1.0 - frac) * gp[j]));
|
|
peq[i] = pow (10.0, 0.05 * (frac * ep[j + 1] + (1.0 - frac) * ep[j]));
|
|
cfc_gain[i] = precomplin * comp[i];
|
|
}
|
|
|
|
delete[] sary;
|
|
}
|
|
|
|
void CFCOMP::calc_cfcomp()
|
|
{
|
|
incr = fsize / ovrlp;
|
|
if (fsize > bsize)
|
|
iasize = fsize;
|
|
else
|
|
iasize = bsize + fsize - incr;
|
|
iainidx = 0;
|
|
iaoutidx = 0;
|
|
if (fsize > bsize)
|
|
{
|
|
if (bsize > incr) oasize = bsize;
|
|
else oasize = incr;
|
|
oainidx = (fsize - bsize - incr) % oasize;
|
|
}
|
|
else
|
|
{
|
|
oasize = bsize;
|
|
oainidx = fsize - incr;
|
|
}
|
|
init_oainidx = oainidx;
|
|
oaoutidx = 0;
|
|
msize = fsize / 2 + 1;
|
|
window.resize(fsize);
|
|
inaccum.resize(iasize);
|
|
forfftin.resize(fsize);
|
|
forfftout.resize(msize * 2);
|
|
cmask.resize(msize);
|
|
mask.resize(msize);
|
|
cfc_gain.resize(msize);
|
|
revfftin.resize(msize * 2);
|
|
revfftout.resize(fsize);
|
|
save.resize(ovrlp);
|
|
for (int i = 0; i < ovrlp; i++)
|
|
save[i].resize(fsize);
|
|
outaccum.resize(oasize);
|
|
nsamps = 0;
|
|
saveidx = 0;
|
|
Rfor = fftwf_plan_dft_r2c_1d(fsize, forfftin.data(), (fftwf_complex *)forfftout.data(), FFTW_ESTIMATE);
|
|
Rrev = fftwf_plan_dft_c2r_1d(fsize, (fftwf_complex *)revfftin.data(), revfftout.data(), FFTW_ESTIMATE);
|
|
calc_cfcwindow();
|
|
|
|
pregain = (2.0 * winfudge) / (double)fsize;
|
|
postgain = 0.5 / ((double)ovrlp * winfudge);
|
|
|
|
fp.resize(nfreqs + 2);
|
|
gp.resize(nfreqs + 2);
|
|
ep.resize(nfreqs + 2);
|
|
comp.resize(msize);
|
|
peq.resize(msize);
|
|
calc_comp();
|
|
|
|
gain = 0.0;
|
|
mmult = exp (-1.0 / (rate * ovrlp * mtau));
|
|
dmult = exp (-(float)fsize / (rate * ovrlp * dtau));
|
|
|
|
delta.resize(msize);
|
|
delta_copy.resize(msize);
|
|
cfc_gain_copy.resize(msize);
|
|
}
|
|
|
|
void CFCOMP::decalc_cfcomp()
|
|
{
|
|
fftwf_destroy_plan(Rrev);
|
|
fftwf_destroy_plan(Rfor);
|
|
}
|
|
|
|
CFCOMP::CFCOMP(
|
|
int _run,
|
|
int _position,
|
|
int _peq_run,
|
|
int _size,
|
|
float* _in,
|
|
float* _out,
|
|
int _fsize,
|
|
int _ovrlp,
|
|
int _rate,
|
|
int _wintype,
|
|
int _comp_method,
|
|
int _nfreqs,
|
|
double _precomp,
|
|
double _prepeq,
|
|
const double* _F,
|
|
const double* _G,
|
|
const double* _E,
|
|
double _mtau,
|
|
double _dtau
|
|
) :
|
|
run (_run),
|
|
position(_position),
|
|
bsize(_size),
|
|
in(_in),
|
|
out(_out),
|
|
fsize(_fsize),
|
|
ovrlp(_ovrlp),
|
|
rate(_rate),
|
|
wintype(_wintype),
|
|
comp_method(_comp_method),
|
|
nfreqs(_nfreqs),
|
|
precomp(_precomp),
|
|
peq_run(_peq_run),
|
|
prepeq(_prepeq),
|
|
mtau(_mtau), // compression metering time constant
|
|
dtau(_dtau) // compression display time constant
|
|
{
|
|
F.resize(nfreqs);
|
|
G.resize(nfreqs);
|
|
E.resize(nfreqs);
|
|
std::copy(_F, _F + nfreqs, F.begin());
|
|
std::copy(_G, _G + nfreqs, G.begin());
|
|
std::copy(_E, _E + nfreqs, E.begin());
|
|
calc_cfcomp();
|
|
}
|
|
|
|
CFCOMP::~CFCOMP()
|
|
{
|
|
decalc_cfcomp();
|
|
}
|
|
|
|
void CFCOMP::flush()
|
|
{
|
|
std::fill(inaccum.begin(), inaccum.end(), 0);
|
|
for (int i = 0; i < ovrlp; i++)
|
|
std::fill(save[i].begin(), save[i].end(), 0);
|
|
std::fill(outaccum.begin(), outaccum.end(), 0);
|
|
nsamps = 0;
|
|
iainidx = 0;
|
|
iaoutidx = 0;
|
|
oainidx = init_oainidx;
|
|
oaoutidx = 0;
|
|
saveidx = 0;
|
|
gain = 0.0;
|
|
std::fill(delta.begin(), delta.end(), 0);
|
|
}
|
|
|
|
|
|
|
|
void CFCOMP::calc_mask()
|
|
{
|
|
int i;
|
|
double _comp;
|
|
double _mask;
|
|
double _delta;
|
|
if (comp_method == 0)
|
|
{
|
|
double mag;
|
|
double test;
|
|
for (i = 0; i < msize; i++)
|
|
{
|
|
mag = sqrt (forfftout[2 * i + 0] * forfftout[2 * i + 0]
|
|
+ forfftout[2 * i + 1] * forfftout[2 * i + 1]);
|
|
_comp = cfc_gain[i];
|
|
test = _comp * mag;
|
|
if (test > 1.0)
|
|
_mask = 1.0 / mag;
|
|
else
|
|
_mask = _comp;
|
|
cmask[i] = _mask;
|
|
if (test > gain) gain = test;
|
|
else gain = mmult * gain;
|
|
|
|
_delta = cfc_gain[i] - cmask[i];
|
|
if (_delta > delta[i]) delta[i] = _delta;
|
|
else delta[i] *= dmult;
|
|
}
|
|
}
|
|
if (peq_run)
|
|
{
|
|
for (i = 0; i < msize; i++)
|
|
{
|
|
mask[i] = cmask[i] * prepeqlin * peq[i];
|
|
}
|
|
}
|
|
else
|
|
std::copy(cmask.begin(), cmask.end(), mask.begin());
|
|
mask_ready = 1;
|
|
}
|
|
|
|
void CFCOMP::execute(int pos)
|
|
{
|
|
if (run && pos == position)
|
|
{
|
|
int i;
|
|
int j;
|
|
int k;
|
|
int sbuff;
|
|
int sbegin;
|
|
for (i = 0; i < 2 * bsize; i += 2)
|
|
{
|
|
inaccum[iainidx] = in[i];
|
|
iainidx = (iainidx + 1) % iasize;
|
|
}
|
|
nsamps += bsize;
|
|
while (nsamps >= fsize)
|
|
{
|
|
for (i = 0, j = iaoutidx; i < fsize; i++, j = (j + 1) % iasize)
|
|
forfftin[i] = (float) (pregain * window[i] * inaccum[j]);
|
|
iaoutidx = (iaoutidx + incr) % iasize;
|
|
nsamps -= incr;
|
|
fftwf_execute (Rfor);
|
|
calc_mask();
|
|
for (i = 0; i < msize; i++)
|
|
{
|
|
revfftin[2 * i + 0] = (float) (mask[i] * forfftout[2 * i + 0]);
|
|
revfftin[2 * i + 1] = (float) (mask[i] * forfftout[2 * i + 1]);
|
|
}
|
|
fftwf_execute (Rrev);
|
|
for (i = 0; i < fsize; i++)
|
|
save[saveidx][i] = postgain * window[i] * revfftout[i];
|
|
for (i = ovrlp; i > 0; i--)
|
|
{
|
|
sbuff = (saveidx + i) % ovrlp;
|
|
sbegin = incr * (ovrlp - i);
|
|
for (j = sbegin, k = oainidx; j < incr + sbegin; j++, k = (k + 1) % oasize)
|
|
{
|
|
if ( i == ovrlp)
|
|
outaccum[k] = save[sbuff][j];
|
|
else
|
|
outaccum[k] += save[sbuff][j];
|
|
}
|
|
}
|
|
saveidx = (saveidx + 1) % ovrlp;
|
|
oainidx = (oainidx + incr) % oasize;
|
|
}
|
|
for (i = 0; i < bsize; i++)
|
|
{
|
|
out[2 * i + 0] = (float) (outaccum[oaoutidx]);
|
|
out[2 * i + 1] = 0.0;
|
|
oaoutidx = (oaoutidx + 1) % oasize;
|
|
}
|
|
}
|
|
else if (out != in)
|
|
std::copy(in, in + bsize * 2, out);
|
|
}
|
|
|
|
void CFCOMP::setBuffers(float* _in, float* _out)
|
|
{
|
|
in = _in;
|
|
out = _out;
|
|
}
|
|
|
|
void CFCOMP::setSamplerate(int _rate)
|
|
{
|
|
decalc_cfcomp();
|
|
rate = _rate;
|
|
calc_cfcomp();
|
|
}
|
|
|
|
void CFCOMP::setSize(int size)
|
|
{
|
|
decalc_cfcomp();
|
|
bsize = size;
|
|
calc_cfcomp();
|
|
}
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* TXA Properties *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
void CFCOMP::setRun(int _run)
|
|
{
|
|
if (run != _run) {
|
|
run = _run;
|
|
}
|
|
}
|
|
|
|
void CFCOMP::setPosition(int pos)
|
|
{
|
|
if (position != pos) {
|
|
position = pos;
|
|
}
|
|
}
|
|
|
|
void CFCOMP::setProfile(int _nfreqs, const double* _F, const double* _G, const double* _E)
|
|
{
|
|
nfreqs = _nfreqs < 1 ? 1 : _nfreqs;
|
|
F.resize(nfreqs);
|
|
G.resize(nfreqs);
|
|
E.resize(nfreqs);
|
|
std::copy(_F, _F + nfreqs, F.begin());
|
|
std::copy(_G, _G + nfreqs, G.begin());
|
|
std::copy(_E, _E + nfreqs, E.begin());
|
|
fp.resize(nfreqs + 2);
|
|
gp.resize(nfreqs + 2);
|
|
ep.resize(nfreqs + 2);
|
|
calc_comp();
|
|
}
|
|
|
|
void CFCOMP::setPrecomp(double _precomp)
|
|
{
|
|
if (precomp != _precomp)
|
|
{
|
|
precomp = _precomp;
|
|
precomplin = pow (10.0, 0.05 * precomp);
|
|
|
|
for (int i = 0; i < msize; i++)
|
|
{
|
|
cfc_gain[i] = precomplin * comp[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
void CFCOMP::setPeqRun(int _run)
|
|
{
|
|
if (peq_run != _run) {
|
|
peq_run = _run;
|
|
}
|
|
}
|
|
|
|
void CFCOMP::setPrePeq(double _prepeq)
|
|
{
|
|
prepeq = _prepeq;
|
|
prepeqlin = pow (10.0, 0.05 * prepeq);
|
|
}
|
|
|
|
void CFCOMP::getDisplayCompression(double* comp_values, int* ready)
|
|
{
|
|
if ((*ready = mask_ready))
|
|
{
|
|
std::copy(delta.begin(), delta.end(), delta_copy.begin());
|
|
std::copy(cfc_gain.begin(), cfc_gain.end(), cfc_gain_copy.begin());
|
|
mask_ready = 0;
|
|
}
|
|
|
|
if (*ready)
|
|
{
|
|
for (int i = 0; i < msize; i++)
|
|
comp_values[i] = 20.0 * MemLog::mlog10 (cfc_gain_copy[i] / (cfc_gain_copy[i] - delta_copy[i]));
|
|
}
|
|
}
|
|
|
|
} // namespace WDSP
|
|
|