VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-09-21 04:16:34 -04:00
Code Issues Projects Releases Wiki Activity

WDSP: FMD and FMSQ: replaced static methods

Browse Source
This commit is contained in:
f4exb 2024-07-26 19:10:53 +02:00
parent a239fe47e9
commit 8c08f40b54
6 changed files with 434 additions and 468 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
plugins/channelrx/wdsprx/wdsprxsink.cpp
View File

@ -649,29 +649,29 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force)
// FM options
if ((m_settings.m_fmDeviation != settings.m_fmDeviation) || force) {
WDSP::FMD::SetFMDeviation(*m_rxa, settings.m_fmDeviation);
m_rxa->fmd->setDeviation(settings.m_fmDeviation);
}
if ((m_settings.m_fmAFLow != settings.m_fmAFLow)
|| (m_settings.m_fmAFHigh != settings.m_fmAFHigh) || force)
{
WDSP::FMD::SetFMAFFilter(*m_rxa, settings.m_fmAFLow, settings.m_fmAFHigh);
m_rxa->fmd->setAFFilter(settings.m_fmAFLow, settings.m_fmAFHigh);
}
if ((m_settings.m_fmAFLimiter != settings.m_fmAFLimiter) || force) {
WDSP::FMD::SetFMLimRun(*m_rxa, settings.m_fmAFLimiter ? 1 : 0);
m_rxa->fmd->setLimRun(settings.m_fmAFLimiter ? 1 : 0);
}
if ((m_settings.m_fmAFLimiterGain != settings.m_fmAFLimiterGain) || force) {
WDSP::FMD::SetFMLimGain(*m_rxa, settings.m_fmAFLimiterGain);
m_rxa->fmd->setLimGain(settings.m_fmAFLimiterGain);
}
if ((m_settings.m_fmCTCSSNotch != settings.m_fmCTCSSNotch) || force) {
WDSP::FMD::SetCTCSSRun(*m_rxa, settings.m_fmCTCSSNotch ? 1 : 0);
m_rxa->fmd->setCTCSSRun(settings.m_fmCTCSSNotch ? 1 : 0);
}
if ((m_settings.m_fmCTCSSNotchFrequency != settings.m_fmCTCSSNotchFrequency) || force) {
WDSP::FMD::SetCTCSSFreq(*m_rxa, settings.m_fmCTCSSNotchFrequency);
m_rxa->fmd->setCTCSSFreq(settings.m_fmCTCSSNotchFrequency);
}
// Squelch
@ -682,7 +682,7 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force)
{
WDSP::SSQL::SetSSQLRun(*m_rxa, 0);
m_rxa->amsq->setRun(0);
WDSP::FMSQ::SetFMSQRun(*m_rxa, 0);
m_rxa->fmsq->setRun(0);
if (settings.m_squelch)
{
@ -704,10 +704,10 @@ void WDSPRxSink::applySettings(const WDSPRxSettings& settings, bool force)
break;
case WDSPRxProfile::SquelchModeFM:
{
WDSP::FMSQ::SetFMSQRun(*m_rxa, 1);
m_rxa->fmsq->setRun(1);
double threshold = pow(10.0, -2.0 * ((double) settings.m_squelchThreshold) / 100.0);
qDebug("WDSPRxSink::applySettings: FM squelch %lf", threshold);
WDSP::FMSQ::SetFMSQThreshold(*m_rxa, threshold);
m_rxa->fmsq->setThreshold(threshold);
}
break;
default:

52
wdsp/RXA.cpp
View File

@ -261,7 +261,7 @@ RXA* RXA::create_rxa (
1.4); // tauI
// FM demodulator
rxa->fmd = FMD::create_fmd (
rxa->fmd = new FMD(
0, // run
rxa->dsp_size, // buffer size
rxa->midbuff, // pointer to input buffer
@ -284,7 +284,7 @@ RXA* RXA::create_rxa (
0); // min phase flag for audio cutoff filter
// FM squelch apply
rxa->fmsq = FMSQ::create_fmsq (
rxa->fmsq = new FMSQ(
0, // run
rxa->dsp_size, // buffer size
rxa->midbuff, // pointer to input signal buffer
@ -578,8 +578,8 @@ void RXA::destroy_rxa (RXA *rxa)
ANF::destroy_anf (rxa->anf);
EQP::destroy_eqp (rxa->eqp);
SNBA::destroy_snba (rxa->snba);
FMSQ::destroy_fmsq (rxa->fmsq);
FMD::destroy_fmd (rxa->fmd);
delete (rxa->fmsq);
delete (rxa->fmd);
delete (rxa->amd);
delete (rxa->amsq);
delete (rxa->smeter);
@ -614,8 +614,8 @@ void RXA::flush_rxa (RXA *rxa)
rxa->smeter->flush();
rxa->amsq->flush();
rxa->amd->flush();
FMD::flush_fmd (rxa->fmd);
FMSQ::flush_fmsq (rxa->fmsq);
rxa->fmd->flush();
rxa->fmsq->flush();
SNBA::flush_snba (rxa->snba);
EQP::flush_eqp (rxa->eqp);
ANF::flush_anf (rxa->anf);
@ -647,8 +647,8 @@ void RXA::xrxa (RXA *rxa)
rxa->amsq->xcap();
rxa->bpsnba->exec_out(0);
rxa->amd->execute();
FMD::xfmd (rxa->fmd);
FMSQ::xfmsq (rxa->fmsq);
rxa->fmd->execute();
rxa->fmsq->execute();
rxa->bpsnba->exec_in(1);
rxa->bpsnba->exec_out(1);
SNBA::xsnba (rxa->snba);
@ -759,9 +759,9 @@ void RXA::setDSPSamplerate (RXA *rxa, int dsp_rate)
rxa->sender->setSamplerate(rxa->dsp_rate);
rxa->amsq->setSamplerate(rxa->dsp_rate);
rxa->amd->setSamplerate(rxa->dsp_rate);
FMD::setSamplerate_fmd (rxa->fmd, rxa->dsp_rate);
FMSQ::setBuffers_fmsq (rxa->fmsq, rxa->midbuff, rxa->midbuff, rxa->fmd->audio);
FMSQ::setSamplerate_fmsq (rxa->fmsq, rxa->dsp_rate);
rxa->fmd->setSamplerate(rxa->dsp_rate);
rxa->fmsq->setBuffers(rxa->midbuff, rxa->midbuff, rxa->fmd->audio);
rxa->fmsq->setSamplerate(rxa->dsp_rate);
SNBA::setSamplerate_snba (rxa->snba, rxa->dsp_rate);
EQP::setSamplerate_eqp (rxa->eqp, rxa->dsp_rate);
ANF::setSamplerate_anf (rxa->anf, rxa->dsp_rate);
@ -829,10 +829,10 @@ void RXA::setDSPBuffsize (RXA *rxa, int dsp_size)
rxa->amsq->setSize(rxa->dsp_size);
rxa->amd->setBuffers(rxa->midbuff, rxa->midbuff);
rxa->amd->setSize(rxa->dsp_size);
FMD::setBuffers_fmd (rxa->fmd, rxa->midbuff, rxa->midbuff);
FMD::setSize_fmd (rxa->fmd, rxa->dsp_size);
FMSQ::setBuffers_fmsq (rxa->fmsq, rxa->midbuff, rxa->midbuff, rxa->fmd->audio);
FMSQ::setSize_fmsq (rxa->fmsq, rxa->dsp_size);
rxa->fmd->setBuffers(rxa->midbuff, rxa->midbuff);
rxa->fmd->setSize(rxa->dsp_size);
rxa->fmsq->setBuffers(rxa->midbuff, rxa->midbuff, rxa->fmd->audio);
rxa->fmsq->setSize(rxa->dsp_size);
SNBA::setBuffers_snba (rxa->snba, rxa->midbuff, rxa->midbuff);
SNBA::setSize_snba (rxa->snba, rxa->dsp_size);
EQP::setBuffers_eqp (rxa->eqp, rxa->midbuff, rxa->midbuff);
@ -1225,22 +1225,20 @@ void RXA::NBPSetAutoIncrease (RXA& rxa, int autoincr)
}
}
void RXA::SetAMDRun(RXA& rxa, int run)
void RXA::SetAMDRun(RXA& rxa, int _run)
{
AMD *a = rxa.amd;
if (run != run)
if (rxa.amd->run != _run)
{
RXA::bp1Check (
rxa,
run,
_run,
rxa.snba->run,
rxa.emnr->run,
rxa.anf->run,
rxa.anr->run
);
run = run;
rxa.amd->run = _run;
RXA::bp1Set (rxa);
}
}
@ -1265,9 +1263,9 @@ void RXA::SetNC (RXA& rxa, int nc)
rxa.bpsnba->SetNC (nc);
BANDPASS::SetBandpassNC (rxa, nc);
EQP::SetEQNC (rxa, nc);
FMSQ::SetFMSQNC (rxa, nc);
FMD::SetFMNCde (rxa, nc);
FMD::SetFMNCaud (rxa, nc);
rxa.fmsq->setNC (nc);
rxa.fmd->setNCde (nc);
rxa.fmd->setNCaud (nc);
rxa.state = oldstate;
}
@ -1277,9 +1275,9 @@ void RXA::SetMP (RXA& rxa, int mp)
rxa.bpsnba->SetMP (mp);
BANDPASS::SetBandpassMP (rxa, mp);
EQP::SetEQMP (rxa, mp);
FMSQ::SetFMSQMP (rxa, mp);
FMD::SetFMMPde (rxa, mp);
FMD::SetFMMPaud (rxa, mp);
rxa.fmsq->setMP (mp);
rxa.fmd->setMPde (mp);
rxa.fmd->setMPaud (mp);
}
} // namespace WDSP

401
wdsp/fmd.cpp
View File

@ -32,42 +32,41 @@ warren@wpratt.com
#include "fir.hpp"
#include "wcpAGC.hpp"
#include "fmd.hpp"
#include "RXA.hpp"
namespace WDSP {
void FMD::calc_fmd (FMD *a)
void FMD::calc()
{
// pll
a->omega_min = TWOPI * a->fmin / a->rate;
a->omega_max = TWOPI * a->fmax / a->rate;
a->g1 = 1.0 - exp(-2.0 * a->omegaN * a->zeta / a->rate);
a->g2 = -a->g1 + 2.0 * (1 - exp(-a->omegaN * a->zeta / a->rate) * cos(a->omegaN / a->rate * sqrt(1.0 - a->zeta * a->zeta)));
a->phs = 0.0;
a->fil_out = 0.0;
a->omega = 0.0;
a->pllpole = a->omegaN * sqrt(2.0 * a->zeta * a->zeta + 1.0 + sqrt((2.0 * a->zeta * a->zeta + 1.0) * (2.0 * a->zeta * a->zeta + 1.0) + 1)) / TWOPI;
omega_min = TWOPI * fmin / rate;
omega_max = TWOPI * fmax / rate;
g1 = 1.0 - exp(-2.0 * omegaN * zeta / rate);
g2 = -g1 + 2.0 * (1 - exp(-omegaN * zeta / rate) * cos(omegaN / rate * sqrt(1.0 - zeta * zeta)));
phs = 0.0;
fil_out = 0.0;
omega = 0.0;
pllpole = omegaN * sqrt(2.0 * zeta * zeta + 1.0 + sqrt((2.0 * zeta * zeta + 1.0) * (2.0 * zeta * zeta + 1.0) + 1)) / TWOPI;
// dc removal
a->mtau = exp(-1.0 / (a->rate * a->tau));
a->onem_mtau = 1.0 - a->mtau;
a->fmdc = 0.0;
mtau = exp(-1.0 / (rate * tau));
onem_mtau = 1.0 - mtau;
fmdc = 0.0;
// pll audio gain
a->again = a->rate / (a->deviation * TWOPI);
again = rate / (deviation * TWOPI);
// CTCSS Removal
a->sntch = SNOTCH::create_snotch(1, a->size, a->out, a->out, (int)a->rate, a->ctcss_freq, 0.0002);
sntch = SNOTCH::create_snotch(1, size, out, out, (int)rate, ctcss_freq, 0.0002);
// detector limiter
a->plim = WCPAGC::create_wcpagc (
plim = WCPAGC::create_wcpagc (
1, // run - always ON
5, // mode
1, // 0 for max(I,Q), 1 for envelope
a->out, // input buff pointer
a->out, // output buff pointer
a->size, // io_buffsize
(int)a->rate, // sample rate
out, // input buff pointer
out, // output buff pointer
size, // io_buffsize
(int)rate, // sample rate
0.001, // tau_attack
0.008, // tau_decay
4, // n_tau
a->lim_gain, // max_gain (sets threshold, initial value)
lim_gain, // max_gain (sets threshold, initial value)
1.0, // var_gain / slope
1.0, // fixed_gain
1.0, // max_input
@ -82,188 +81,185 @@ void FMD::calc_fmd (FMD *a)
0.100); // tau_hang_decay
}
void FMD::decalc_fmd (FMD *a)
void FMD::decalc()
{
WCPAGC::destroy_wcpagc(a->plim);
SNOTCH::destroy_snotch(a->sntch);
WCPAGC::destroy_wcpagc(plim);
SNOTCH::destroy_snotch(sntch);
}
FMD* FMD::create_fmd(
int run,
int size,
float* in,
float* out,
int rate,
double deviation,
double f_low,
double f_high,
double fmin,
double fmax,
double zeta,
double omegaN,
double tau,
double afgain,
int sntch_run,
double ctcss_freq,
int nc_de,
int mp_de,
int nc_aud,
int mp_aud
)
FMD::FMD(
int _run,
int _size,
float* _in,
float* _out,
int _rate,
double _deviation,
double _f_low,
double _f_high,
double _fmin,
double _fmax,
double _zeta,
double _omegaN,
double _tau,
double _afgain,
int _sntch_run,
double _ctcss_freq,
int _nc_de,
int _mp_de,
int _nc_aud,
int _mp_aud
) :
run(_run),
size(_size),
in(_in),
out(_out),
rate((double) _rate),
f_low(_f_low),
f_high(_f_high),
fmin(_fmin),
fmax(_fmax),
zeta(_zeta),
omegaN(_omegaN),
tau(_tau),
deviation(_deviation),
nc_de(_nc_de),
mp_de(_mp_de),
nc_aud(_nc_aud),
mp_aud(_mp_aud),
afgain(_afgain),
sntch_run(_sntch_run),
ctcss_freq(_ctcss_freq),
lim_run(0),
lim_gain(0.0001), // 2.5
lim_pre_gain(0.01) // 0.4
{
FMD *a = new FMD;
float* impulse;
a->run = run;
a->size = size;
a->in = in;
a->out = out;
a->rate = (float)rate;
a->deviation = deviation;
a->f_low = f_low;
a->f_high = f_high;
a->fmin = fmin;
a->fmax = fmax;
a->zeta = zeta;
a->omegaN = omegaN;
a->tau = tau;
a->afgain = afgain;
a->sntch_run = sntch_run;
a->ctcss_freq = ctcss_freq;
a->nc_de = nc_de;
a->mp_de = mp_de;
a->nc_aud = nc_aud;
a->mp_aud = mp_aud;
a->lim_run = 0;
a->lim_pre_gain = 0.01; // 0.4
a->lim_gain = 0.0001; // 2.5
calc_fmd (a);
calc();
// de-emphasis filter
a->audio = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex));
impulse = FCurve::fc_impulse (a->nc_de, a->f_low, a->f_high, +20.0 * log10(a->f_high / a->f_low), 0.0, 1, a->rate, 1.0 / (2.0 * a->size), 0, 0);
a->pde = FIRCORE::create_fircore (a->size, a->audio, a->out, a->nc_de, a->mp_de, impulse);
audio = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
impulse = FCurve::fc_impulse (nc_de, f_low, f_high, +20.0 * log10(f_high / f_low), 0.0, 1, rate, 1.0 / (2.0 * size), 0, 0);
pde = FIRCORE::create_fircore (size, audio, out, nc_de, mp_de, impulse);
delete[] (impulse);
// audio filter
impulse = FIR::fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
a->paud = FIRCORE::create_fircore (a->size, a->out, a->out, a->nc_aud, a->mp_aud, impulse);
impulse = FIR::fir_bandpass(nc_aud, 0.8 * f_low, 1.1 * f_high, rate, 0, 1, afgain / (2.0 * size));
paud = FIRCORE::create_fircore (size, out, out, nc_aud, mp_aud, impulse);
delete[] (impulse);
return a;
}
void FMD::destroy_fmd (FMD *a)
FMD::~FMD()
{
FIRCORE::destroy_fircore (a->paud);
FIRCORE::destroy_fircore (a->pde);
delete[] (a->audio);
decalc_fmd (a);
delete (a);
FIRCORE::destroy_fircore (paud);
FIRCORE::destroy_fircore (pde);
delete[] (audio);
decalc();
}
void FMD::flush_fmd (FMD *a)
void FMD::flush()
{
std::fill(a->audio, a->audio + a->size * 2, 0);
FIRCORE::flush_fircore (a->pde);
FIRCORE::flush_fircore (a->paud);
a->phs = 0.0;
a->fil_out = 0.0;
a->omega = 0.0;
a->fmdc = 0.0;
SNOTCH::flush_snotch (a->sntch);
WCPAGC::flush_wcpagc (a->plim);
std::fill(audio, audio + size * 2, 0);
FIRCORE::flush_fircore (pde);
FIRCORE::flush_fircore (paud);
phs = 0.0;
fil_out = 0.0;
omega = 0.0;
fmdc = 0.0;
SNOTCH::flush_snotch (sntch);
WCPAGC::flush_wcpagc (plim);
}
void FMD::xfmd (FMD *a)
void FMD::execute()
{
if (a->run)
if (run)
{
int i;
double det, del_out;
double vco[2], corr[2];
for (i = 0; i < a->size; i++)
for (i = 0; i < size; i++)
{
// pll
vco[0] = cos (a->phs);
vco[1] = sin (a->phs);
corr[0] = + a->in[2 * i + 0] * vco[0] + a->in[2 * i + 1] * vco[1];
corr[1] = - a->in[2 * i + 0] * vco[1] + a->in[2 * i + 1] * vco[0];
vco[0] = cos (phs);
vco[1] = sin (phs);
corr[0] = + in[2 * i + 0] * vco[0] + in[2 * i + 1] * vco[1];
corr[1] = - in[2 * i + 0] * vco[1] + in[2 * i + 1] * vco[0];
if ((corr[0] == 0.0) && (corr[1] == 0.0)) corr[0] = 1.0;
det = atan2 (corr[1], corr[0]);
del_out = a->fil_out;
a->omega += a->g2 * det;
if (a->omega < a->omega_min) a->omega = a->omega_min;
if (a->omega > a->omega_max) a->omega = a->omega_max;
a->fil_out = a->g1 * det + a->omega;
a->phs += del_out;
while (a->phs >= TWOPI) a->phs -= TWOPI;
while (a->phs < 0.0) a->phs += TWOPI;
del_out = fil_out;
omega += g2 * det;
if (omega < omega_min) omega = omega_min;
if (omega > omega_max) omega = omega_max;
fil_out = g1 * det + omega;
phs += del_out;
while (phs >= TWOPI) phs -= TWOPI;
while (phs < 0.0) phs += TWOPI;
// dc removal, gain, & demod output
a->fmdc = a->mtau * a->fmdc + a->onem_mtau * a->fil_out;
a->audio[2 * i + 0] = a->again * (a->fil_out - a->fmdc);
a->audio[2 * i + 1] = a->audio[2 * i + 0];
fmdc = mtau * fmdc + onem_mtau * fil_out;
audio[2 * i + 0] = again * (fil_out - fmdc);
audio[2 * i + 1] = audio[2 * i + 0];
}
// de-emphasis
FIRCORE::xfircore (a->pde);
FIRCORE::xfircore (pde);
// audio filter
FIRCORE::xfircore (a->paud);
FIRCORE::xfircore (paud);
// CTCSS Removal
SNOTCH::xsnotch (a->sntch);
if (a->lim_run)
SNOTCH::xsnotch (sntch);
if (lim_run)
{
for (i = 0; i < 2 * a->size; i++)
a->out[i] *= a->lim_pre_gain;
WCPAGC::xwcpagc (a->plim);
for (i = 0; i < 2 * size; i++)
out[i] *= lim_pre_gain;
WCPAGC::xwcpagc (plim);
}
}
else if (a->in != a->out)
std::copy( a->in, a->in + a->size * 2, a->out);
else if (in != out)
std::copy( in, in + size * 2, out);
}
void FMD::setBuffers_fmd (FMD *a, float* in, float* out)
void FMD::setBuffers(float* _in, float* _out)
{
decalc_fmd (a);
a->in = in;
a->out = out;
calc_fmd (a);
FIRCORE::setBuffers_fircore (a->pde, a->audio, a->out);
FIRCORE::setBuffers_fircore (a->paud, a->out, a->out);
WCPAGC::setBuffers_wcpagc (a->plim, a->out, a->out);
decalc();
in = _in;
out = _out;
calc();
FIRCORE::setBuffers_fircore (pde, audio, out);
FIRCORE::setBuffers_fircore (paud, out, out);
WCPAGC::setBuffers_wcpagc (plim, out, out);
}
void FMD::setSamplerate_fmd (FMD *a, int rate)
void FMD::setSamplerate(int _rate)
{
float* impulse;
decalc_fmd (a);
a->rate = rate;
calc_fmd (a);
decalc();
rate = _rate;
calc();
// de-emphasis filter
impulse = FCurve::fc_impulse (a->nc_de, a->f_low, a->f_high, +20.0 * log10(a->f_high / a->f_low), 0.0, 1, a->rate, 1.0 / (2.0 * a->size), 0, 0);
FIRCORE::setImpulse_fircore (a->pde, impulse, 1);
impulse = FCurve::fc_impulse (nc_de, f_low, f_high, +20.0 * log10(f_high / f_low), 0.0, 1, rate, 1.0 / (2.0 * size), 0, 0);
FIRCORE::setImpulse_fircore (pde, impulse, 1);
delete[] (impulse);
// audio filter
impulse = FIR::fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
FIRCORE::setImpulse_fircore (a->paud, impulse, 1);
impulse = FIR::fir_bandpass(nc_aud, 0.8 * f_low, 1.1 * f_high, rate, 0, 1, afgain / (2.0 * size));
FIRCORE::setImpulse_fircore (paud, impulse, 1);
delete[] (impulse);
WCPAGC::setSamplerate_wcpagc (a->plim, (int)a->rate);
WCPAGC::setSamplerate_wcpagc (plim, (int)rate);
}
void FMD::setSize_fmd (FMD *a, int size)
void FMD::setSize(int _size)
{
float* impulse;
decalc_fmd (a);
delete[] (a->audio);
a->size = size;
calc_fmd (a);
a->audio = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex));
decalc();
delete[] (audio);
size = _size;
calc();
audio = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
// de-emphasis filter
FIRCORE::destroy_fircore (a->pde);
impulse = FCurve::fc_impulse (a->nc_de, a->f_low, a->f_high, +20.0 * log10(a->f_high / a->f_low), 0.0, 1, a->rate, 1.0 / (2.0 * a->size), 0, 0);
a->pde = FIRCORE::create_fircore (a->size, a->audio, a->out, a->nc_de, a->mp_de, impulse);
FIRCORE::destroy_fircore (pde);
impulse = FCurve::fc_impulse (nc_de, f_low, f_high, +20.0 * log10(f_high / f_low), 0.0, 1, rate, 1.0 / (2.0 * size), 0, 0);
pde = FIRCORE::create_fircore (size, audio, out, nc_de, mp_de, impulse);
delete[] (impulse);
// audio filter
FIRCORE::destroy_fircore (a->paud);
impulse = FIR::fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
a->paud = FIRCORE::create_fircore (a->size, a->out, a->out, a->nc_aud, a->mp_aud, impulse);
FIRCORE::destroy_fircore (paud);
impulse = FIR::fir_bandpass(nc_aud, 0.8 * f_low, 1.1 * f_high, rate, 0, 1, afgain / (2.0 * size));
paud = FIRCORE::create_fircore (size, out, out, nc_aud, mp_aud, impulse);
delete[] (impulse);
WCPAGC::setSize_wcpagc (a->plim, a->size);
WCPAGC::setSize_wcpagc (plim, size);
}
/********************************************************************************************************
@ -272,121 +268,102 @@ void FMD::setSize_fmd (FMD *a, int size)
* *
********************************************************************************************************/
void FMD::SetFMDeviation (RXA& rxa, double deviation)
void FMD::setDeviation(double _deviation)
{
FMD *a;
a = rxa.fmd;
a->deviation = deviation;
a->again = a->rate / (a->deviation * TWOPI);
deviation = _deviation;
again = rate / (deviation * TWOPI);
}
void FMD::SetCTCSSFreq (RXA& rxa, double freq)
void FMD::setCTCSSFreq(double freq)
{
FMD *a;
a = rxa.fmd;
a->ctcss_freq = freq;
SNOTCH::SetSNCTCSSFreq (a->sntch, a->ctcss_freq);
ctcss_freq = freq;
SNOTCH::SetSNCTCSSFreq (sntch, ctcss_freq);
}
void FMD::SetCTCSSRun (RXA& rxa, int run)
void FMD::setCTCSSRun(int run)
{
FMD *a;
a = rxa.fmd;
a->sntch_run = run;
SNOTCH::SetSNCTCSSRun (a->sntch, a->sntch_run);
sntch_run = run;
SNOTCH::SetSNCTCSSRun (sntch, sntch_run);
}
void FMD::SetFMNCde (RXA& rxa, int nc)
void FMD::setNCde(int nc)
{
FMD *a;
float* impulse;
a = rxa.fmd;
if (a->nc_de != nc)
if (nc_de != nc)
{
a->nc_de = nc;
impulse = FCurve::fc_impulse (a->nc_de, a->f_low, a->f_high, +20.0 * log10(a->f_high / a->f_low), 0.0, 1, a->rate, 1.0 / (2.0 * a->size), 0, 0);
FIRCORE::setNc_fircore (a->pde, a->nc_de, impulse);
nc_de = nc;
impulse = FCurve::fc_impulse (nc_de, f_low, f_high, +20.0 * log10(f_high / f_low), 0.0, 1, rate, 1.0 / (2.0 * size), 0, 0);
FIRCORE::setNc_fircore (pde, nc_de, impulse);
delete[] (impulse);
}
}
void FMD::SetFMMPde (RXA& rxa, int mp)
void FMD::setMPde(int mp)
{
FMD *a;
a = rxa.fmd;
if (a->mp_de != mp)
if (mp_de != mp)
{
a->mp_de = mp;
FIRCORE::setMp_fircore (a->pde, a->mp_de);
mp_de = mp;
FIRCORE::setMp_fircore (pde, mp_de);
}
}
void FMD::SetFMNCaud (RXA& rxa, int nc)
void FMD::setNCaud(int nc)
{
FMD *a;
float* impulse;
a = rxa.fmd;
if (a->nc_aud != nc)
if (nc_aud != nc)
{
a->nc_aud = nc;
impulse = FIR::fir_bandpass(a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
FIRCORE::setNc_fircore (a->paud, a->nc_aud, impulse);
nc_aud = nc;
impulse = FIR::fir_bandpass(nc_aud, 0.8 * f_low, 1.1 * f_high, rate, 0, 1, afgain / (2.0 * size));
FIRCORE::setNc_fircore (paud, nc_aud, impulse);
delete[] (impulse);
}
}
void FMD::SetFMMPaud (RXA& rxa, int mp)
void FMD::setMPaud(int mp)
{
FMD *a;
a = rxa.fmd;
if (a->mp_aud != mp)
if (mp_aud != mp)
{
a->mp_aud = mp;
FIRCORE::setMp_fircore (a->paud, a->mp_aud);
mp_aud = mp;
FIRCORE::setMp_fircore (paud, mp_aud);
}
}
void FMD::SetFMLimRun (RXA& rxa, int run)
void FMD::setLimRun(int run)
{
FMD *a;
a = rxa.fmd;
if (a->lim_run != run) {
a->lim_run = run;
if (lim_run != run) {
lim_run = run;
}
}
void FMD::SetFMLimGain (RXA& rxa, double gaindB)
void FMD::setLimGain(double gaindB)
{
double gain = pow(10.0, gaindB / 20.0);
FMD *a = rxa.fmd;
if (a->lim_gain != gain)
if (lim_gain != gain)
{
decalc_fmd(a);
a->lim_gain = gain;
calc_fmd(a);
decalc();
lim_gain = gain;
calc();
}
}
void FMD::SetFMAFFilter(RXA& rxa, double low, double high)
void FMD::setAFFilter(double low, double high)
{
FMD *a = rxa.fmd;
float* impulse;
if (a->f_low != low || a->f_high != high)
if (f_low != low || f_high != high)
{
a->f_low = low;
a->f_high = high;
f_low = low;
f_high = high;
// de-emphasis filter
impulse = FCurve::fc_impulse (a->nc_de, a->f_low, a->f_high, +20.0 * log10(a->f_high / a->f_low), 0.0, 1, a->rate, 1.0 / (2.0 * a->size), 0, 0);
FIRCORE::setImpulse_fircore (a->pde, impulse, 1);
impulse = FCurve::fc_impulse (nc_de, f_low, f_high, +20.0 * log10(f_high / f_low), 0.0, 1, rate, 1.0 / (2.0 * size), 0, 0);
FIRCORE::setImpulse_fircore (pde, impulse, 1);
delete[] (impulse);
// audio filter
impulse = FIR::fir_bandpass (a->nc_aud, 0.8 * a->f_low, 1.1 * a->f_high, a->rate, 0, 1, a->afgain / (2.0 * a->size));
FIRCORE::setImpulse_fircore (a->paud, impulse, 1);
impulse = FIR::fir_bandpass (nc_aud, 0.8 * f_low, 1.1 * f_high, rate, 0, 1, afgain / (2.0 * size));
FIRCORE::setImpulse_fircore (paud, impulse, 1);
delete[] (impulse);
}
}

40
wdsp/fmd.hpp
View File

@ -35,7 +35,6 @@ namespace WDSP {
class FIRCORE;
class SNOTCH;
class WCPAGC;
class RXA;
class WDSP_API FMD
{
@ -87,7 +86,7 @@ public:
double lim_gain;
double lim_pre_gain;
static FMD* create_fmd (
FMD(
int run,
int size,
float* in,
@ -109,27 +108,28 @@ public:
int nc_aud,
int mp_aud
);
static void destroy_fmd (FMD *a);
static void flush_fmd (FMD *a);
static void xfmd (FMD *a);
static void setBuffers_fmd (FMD *a, float* in, float* out);
static void setSamplerate_fmd (FMD *a, int rate);
static void setSize_fmd (FMD *a, int size);
~FMD();
void flush();
void execute();
void setBuffers(float* in, float* out);
void setSamplerate(int rate);
void setSize(int size);
// RXA Properties
static void SetFMDeviation (RXA& rxa, double deviation);
static void SetCTCSSFreq (RXA& rxa, double freq);
static void SetCTCSSRun (RXA& rxa, int run);
static void SetFMNCde (RXA& rxa, int nc);
static void SetFMMPde (RXA& rxa, int mp);
static void SetFMNCaud (RXA& rxa, int nc);
static void SetFMMPaud (RXA& rxa, int mp);
static void SetFMLimRun (RXA& rxa, int run);
static void SetFMLimGain (RXA& rxa, double gaindB);
static void SetFMAFFilter(RXA& rxa, double low, double high);
void setDeviation(double deviation);
void setCTCSSFreq(double freq);
void setCTCSSRun(int run);
void setNCde(int nc);
void setMPde(int mp);
void setNCaud(int nc);
void setMPaud(int mp);
void setLimRun(int run);
void setLimGain(double gaindB);
void setAFFilter(double low, double high);
private:
static void calc_fmd (FMD *a);
static void decalc_fmd (FMD *a);
void calc();
void decalc();
};
} // namespace WDSP

323
wdsp/fmsq.cpp
View File

@ -29,132 +29,128 @@ warren@wpratt.com
#include "fircore.hpp"
#include "eq.hpp"
#include "fmsq.hpp"
#include "RXA.hpp"
namespace WDSP {
void FMSQ::calc_fmsq (FMSQ *a)
void FMSQ::calc()
{
double delta, theta;
float* impulse;
int i;
// noise filter
a->noise = new float[2 * a->size * 2]; // (float *)malloc0(2 * a->size * sizeof(complex));
a->F[0] = 0.0;
a->F[1] = a->fc;
a->F[2] = *a->pllpole;
a->F[3] = 20000.0;
a->G[0] = 0.0;
a->G[1] = 0.0;
a->G[2] = 3.0;
a->G[3] = +20.0 * log10(20000.0 / *a->pllpole);
impulse = EQP::eq_impulse (a->nc, 3, a->F, a->G, a->rate, 1.0 / (2.0 * a->size), 0, 0);
a->p = FIRCORE::create_fircore (a->size, a->trigger, a->noise, a->nc, a->mp, impulse);
noise = new float[2 * size * 2]; // (float *)malloc0(2 * size * sizeof(complex));
F[0] = 0.0;
F[1] = fc;
F[2] = *pllpole;
F[3] = 20000.0;
G[0] = 0.0;
G[1] = 0.0;
G[2] = 3.0;
G[3] = +20.0 * log10(20000.0 / *pllpole);
impulse = EQP::eq_impulse (nc, 3, F, G, rate, 1.0 / (2.0 * size), 0, 0);
p = FIRCORE::create_fircore (size, trigger, noise, nc, mp, impulse);
delete[] (impulse);
// noise averaging
a->avm = exp(-1.0 / (a->rate * a->avtau));
a->onem_avm = 1.0 - a->avm;
a->avnoise = 100.0;
a->longavm = exp(-1.0 / (a->rate * a->longtau));
a->onem_longavm = 1.0 - a->longavm;
a->longnoise = 1.0;
avm = exp(-1.0 / (rate * avtau));
onem_avm = 1.0 - avm;
avnoise = 100.0;
longavm = exp(-1.0 / (rate * longtau));
onem_longavm = 1.0 - longavm;
longnoise = 1.0;
// level change
a->ntup = (int)(a->tup * a->rate);
a->ntdown = (int)(a->tdown * a->rate);
a->cup = new double[a->ntup + 1]; // (float *)malloc0 ((a->ntup + 1) * sizeof(float));
a->cdown = new double[a->ntdown + 1]; //(float *)malloc0 ((a->ntdown + 1) * sizeof(float));
delta = PI / (double) a->ntup;
ntup = (int)(tup * rate);
ntdown = (int)(tdown * rate);
cup = new double[ntup + 1]; // (float *)malloc0 ((ntup + 1) * sizeof(float));
cdown = new double[ntdown + 1]; //(float *)malloc0 ((ntdown + 1) * sizeof(float));
delta = PI / (double) ntup;
theta = 0.0;
for (i = 0; i <= a->ntup; i++)
for (i = 0; i <= ntup; i++)
{
a->cup[i] = 0.5 * (1.0 - cos(theta));
cup[i] = 0.5 * (1.0 - cos(theta));
theta += delta;
}
delta = PI / (double) a->ntdown;
delta = PI / (double) ntdown;
theta = 0.0;
for (i = 0; i <= a->ntdown; i++)
for (i = 0; i <= ntdown; i++)
{
a->cdown[i] = 0.5 * (1 + cos(theta));
cdown[i] = 0.5 * (1 + cos(theta));
theta += delta;
}
// control
a->state = 0;
a->ready = 0;
a->ramp = 0.0;
a->rstep = 1.0 / a->rate;
state = 0;
ready = 0;
ramp = 0.0;
rstep = 1.0 / rate;
}
void FMSQ::decalc_fmsq (FMSQ *a)
void FMSQ::decalc()
{
delete[] (a->cdown);
delete[] (a->cup);
FIRCORE::destroy_fircore (a->p);
delete[] (a->noise);
delete[] (cdown);
delete[] (cup);
FIRCORE::destroy_fircore (p);
delete[] (noise);
}
FMSQ* FMSQ::create_fmsq (
int run,
int size,
float* insig,
float* outsig,
float* trigger,
int rate,
double fc,
double* pllpole,
double tdelay,
double avtau,
double longtau,
double tup,
double tdown,
double tail_thresh,
double unmute_thresh,
double min_tail,
double max_tail,
int nc,
int mp
)
FMSQ::FMSQ(
int _run,
int _size,
float* _insig,
float* _outsig,
float* _trigger,
int _rate,
double _fc,
double* _pllpole,
double _tdelay,
double _avtau,
double _longtau,
double _tup,
double _tdown,
double _tail_thresh,
double _unmute_thresh,
double _min_tail,
double _max_tail,
int _nc,
int _mp
) :
run(_run),
size(_size),
insig(_insig),
outsig(_outsig),
trigger(_trigger),
rate((double) _rate),
fc(_fc),
pllpole(_pllpole),
tdelay(_tdelay),
avtau(_avtau),
longtau(_longtau),
tup(_tup),
tdown(_tdown),
tail_thresh(_tail_thresh),
unmute_thresh(_unmute_thresh),
min_tail(_min_tail),
max_tail(_max_tail),
nc(_nc),
mp(_mp)
{
FMSQ *a = new FMSQ;
a->run = run;
a->size = size;
a->insig = insig;
a->outsig = outsig;
a->trigger = trigger;
a->rate = (float)rate;
a->fc = fc;
a->pllpole = pllpole;
a->tdelay = tdelay;
a->avtau = avtau;
a->longtau = longtau;
a->tup = tup;
a->tdown = tdown;
a->tail_thresh = tail_thresh;
a->unmute_thresh = unmute_thresh;
a->min_tail = min_tail;
a->max_tail = max_tail;
a->nc = nc;
a->mp = mp;
calc_fmsq (a);
return a;
calc();
}
void FMSQ::destroy_fmsq (FMSQ *a)
FMSQ::~FMSQ()
{
decalc_fmsq (a);
delete (a);
decalc();
}
void FMSQ::flush_fmsq (FMSQ *a)
void FMSQ::flush()
{
FIRCORE::flush_fircore (a->p);
a->avnoise = 100.0;
a->longnoise = 1.0;
a->state = 0;
a->ready = 0;
a->ramp = 0.0;
FIRCORE::flush_fircore (p);
avnoise = 100.0;
longnoise = 1.0;
state = 0;
ready = 0;
ramp = 0.0;
}
enum _fmsqstate
@ -166,120 +162,120 @@ enum _fmsqstate
DECREASE
};
void FMSQ::xfmsq (FMSQ *a)
void FMSQ::execute()
{
if (a->run)
if (run)
{
int i;
double noise, lnlimit;
FIRCORE::xfircore (a->p);
double _noise, lnlimit;
FIRCORE::xfircore (p);
for (i = 0; i < a->size; i++)
for (i = 0; i < size; i++)
{
double noise0 = a->noise[2 * i + 0];
double noise1 = a->noise[2 * i + 1];
noise = sqrt(noise0 * noise0 + noise1 * noise1);
a->avnoise = a->avm * a->avnoise + a->onem_avm * noise;
a->longnoise = a->longavm * a->longnoise + a->onem_longavm * noise;
double noise0 = noise[2 * i + 0];
double noise1 = noise[2 * i + 1];
_noise = sqrt(noise0 * noise0 + noise1 * noise1);
avnoise = avm * avnoise + onem_avm * _noise;
longnoise = longavm * longnoise + onem_longavm * _noise;
if (!a->ready)
a->ramp += a->rstep;
if (!ready)
ramp += rstep;
if (a->ramp >= a->tdelay)
a->ready = 1;
if (ramp >= tdelay)
ready = 1;
switch (a->state)
switch (state)
{
case MUTED:
if (a->avnoise < a->unmute_thresh && a->ready)
if (avnoise < unmute_thresh && ready)
{
a->state = INCREASE;
a->count = a->ntup;
state = INCREASE;
count = ntup;
}
a->outsig[2 * i + 0] = 0.0;
a->outsig[2 * i + 1] = 0.0;
outsig[2 * i + 0] = 0.0;
outsig[2 * i + 1] = 0.0;
break;
case INCREASE:
a->outsig[2 * i + 0] = a->insig[2 * i + 0] * a->cup[a->ntup - a->count];
a->outsig[2 * i + 1] = a->insig[2 * i + 1] * a->cup[a->ntup - a->count];
outsig[2 * i + 0] = insig[2 * i + 0] * cup[ntup - count];
outsig[2 * i + 1] = insig[2 * i + 1] * cup[ntup - count];
if (a->count-- == 0)
a->state = UNMUTED;
if (count-- == 0)
state = UNMUTED;
break;
case UNMUTED:
if (a->avnoise > a->tail_thresh)
if (avnoise > tail_thresh)
{
a->state = TAIL;
state = TAIL;
if ((lnlimit = a->longnoise) > 1.0)
if ((lnlimit = longnoise) > 1.0)
lnlimit = 1.0;
a->count = (int)((a->min_tail + (a->max_tail - a->min_tail) * lnlimit) * a->rate);
count = (int)((min_tail + (max_tail - min_tail) * lnlimit) * rate);
}
a->outsig[2 * i + 0] = a->insig[2 * i + 0];
a->outsig[2 * i + 1] = a->insig[2 * i + 1];
outsig[2 * i + 0] = insig[2 * i + 0];
outsig[2 * i + 1] = insig[2 * i + 1];
break;
case TAIL:
a->outsig[2 * i + 0] = a->insig[2 * i + 0];
a->outsig[2 * i + 1] = a->insig[2 * i + 1];
outsig[2 * i + 0] = insig[2 * i + 0];
outsig[2 * i + 1] = insig[2 * i + 1];
if (a->avnoise < a->unmute_thresh)
if (avnoise < unmute_thresh)
{
a->state = UNMUTED;
state = UNMUTED;
}
else if (a->count-- == 0)
else if (count-- == 0)
{
a->state = DECREASE;
a->count = a->ntdown;
state = DECREASE;
count = ntdown;
}
break;
case DECREASE:
a->outsig[2 * i + 0] = a->insig[2 * i + 0] * a->cdown[a->ntdown - a->count];
a->outsig[2 * i + 1] = a->insig[2 * i + 1] * a->cdown[a->ntdown - a->count];
outsig[2 * i + 0] = insig[2 * i + 0] * cdown[ntdown - count];
outsig[2 * i + 1] = insig[2 * i + 1] * cdown[ntdown - count];
if (a->count-- == 0)
a->state = MUTED;
if (count-- == 0)
state = MUTED;
break;
}
}
}
else if (a->insig != a->outsig)
else if (insig != outsig)
{
std::copy(a->insig, a->insig + a->size * 2, a->outsig);
std::copy(insig, insig + size * 2, outsig);
}
}
void FMSQ::setBuffers_fmsq (FMSQ *a, float* in, float* out, float* trig)
void FMSQ::setBuffers(float* in, float* out, float* trig)
{
a->insig = in;
a->outsig = out;
a->trigger = trig;
FIRCORE::setBuffers_fircore (a->p, a->trigger, a->noise);
insig = in;
outsig = out;
trigger = trig;
FIRCORE::setBuffers_fircore (p, trigger, noise);
}
void FMSQ::setSamplerate_fmsq (FMSQ *a, int rate)
void FMSQ::setSamplerate(int _rate)
{
decalc_fmsq (a);
a->rate = rate;
calc_fmsq (a);
decalc();
rate = _rate;
calc();
}
void FMSQ::setSize_fmsq (FMSQ *a, int size)
void FMSQ::setSize(int _size)
{
decalc_fmsq (a);
a->size = size;
calc_fmsq (a);
decalc();
size = _size;
calc();
}
/********************************************************************************************************
@ -288,41 +284,36 @@ void FMSQ::setSize_fmsq (FMSQ *a, int size)
* *
********************************************************************************************************/
void FMSQ::SetFMSQRun (RXA& rxa, int run)
void FMSQ::setRun(int _run)
{
rxa.fmsq->run = run;
run = _run;
}
void FMSQ::SetFMSQThreshold (RXA& rxa, double threshold)
void FMSQ::setThreshold(double threshold)
{
rxa.fmsq->tail_thresh = threshold;
rxa.fmsq->unmute_thresh = 0.9 * threshold;
tail_thresh = threshold;
unmute_thresh = 0.9 * threshold;
}
void FMSQ::SetFMSQNC (RXA& rxa, int nc)
void FMSQ::setNC(int _nc)
{
FMSQ *a;
float* impulse;
a = rxa.fmsq;
if (a->nc != nc)
if (nc != _nc)
{
a->nc = nc;
impulse = EQP::eq_impulse (a->nc, 3, a->F, a->G, a->rate, 1.0 / (2.0 * a->size), 0, 0);
FIRCORE::setNc_fircore (a->p, a->nc, impulse);
nc = _nc;
impulse = EQP::eq_impulse (nc, 3, F, G, rate, 1.0 / (2.0 * size), 0, 0);
FIRCORE::setNc_fircore (p, nc, impulse);
delete[] (impulse);
}
}
void FMSQ::SetFMSQMP (RXA& rxa, int mp)
void FMSQ::setMP(int _mp)
{
FMSQ *a;
a = rxa.fmsq;
if (a->mp != mp)
if (mp != _mp)
{
a->mp = mp;
FIRCORE::setMp_fircore (a->p, a->mp);
mp = _mp;
FIRCORE::setMp_fircore (p, mp);
}
}

68
wdsp/fmsq.hpp
View File

@ -33,7 +33,6 @@ warren@wpratt.com
namespace WDSP {
class FIRCORE;
class RXA;
class WDSP_API FMSQ
{
@ -77,42 +76,43 @@ public:
int mp;
FIRCORE *p;
static FMSQ* create_fmsq (
int run,
int size,
float* insig,
float* outsig,
float* trigger,
int rate,
double fc,
double* pllpole,
double tdelay,
double avtau,
double longtau,
double tup,
double tdown,
double tail_thresh,
double unmute_thresh,
double min_tail,
double max_tail,
int nc,
int mp
FMSQ(
int _run,
int _size,
float* _insig,
float* _outsig,
float* _trigger,
int _rate,
double _fc,
double* _pllpole,
double _tdelay,
double _avtau,
double _longtau,
double _tup,
double _tdown,
double _tail_thresh,
double _unmute_thresh,
double _min_tail,
double _max_tail,
int _nc,
int _mp
);
static void destroy_fmsq (FMSQ *a);
static void flush_fmsq (FMSQ *a);
static void xfmsq (FMSQ *a);
static void setBuffers_fmsq (FMSQ *a, float* in, float* out, float* trig);
static void setSamplerate_fmsq (FMSQ *a, int rate);
static void setSize_fmsq (FMSQ *a, int size);
// RXA Properties
static void SetFMSQRun (RXA& rxa, int run);
static void SetFMSQThreshold (RXA& rxa, double threshold);
static void SetFMSQNC (RXA& rxa, int nc);
static void SetFMSQMP (RXA& rxa, int mp);
~FMSQ();
void flush();
void execute();
void setBuffers(float* in, float* out, float* trig);
void setSamplerate(int rate);
void setSize(int size);
// Public Properties
void setRun(int run);
void setThreshold(double threshold);
void setNC(int nc);
void setMP(int mp);
private:
static void calc_fmsq (FMSQ *a);
static void decalc_fmsq (FMSQ *a);
void calc();
void decalc();
};
} // namespace WDSP