VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-12-22 17:45:48 -05:00
Code Issues Projects Releases Wiki Activity
d52179dbe2
sdrangel/wdsp/cfcomp.cpp
f4exb 34917a0b21 WDSP: more rework
2024-08-05 20:05:59 +02:00

495 lines
12 KiB
C++
Raw Blame History

/* 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
Reference in New Issue View Git Blame Copy Permalink