You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
495 lines
12 KiB
495 lines
12 KiB
/* 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), |
|
mask_ready(0), |
|
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 |
|
|
|
|