sdrangel/wdsp/fmsq.cpp

303 wiersze
8.2 KiB
C++

/* fmsq.c
This file is part of a program that implements a Software-Defined Radio.
Copyright (C) 2013, 2016 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 "firmin.hpp"
#include "eq.hpp"
#include "fmsq.hpp"
#include "RXA.hpp"
namespace WDSP {
void FMSQ::calc_fmsq (FMSQ *a)
{
double delta, theta;
double* impulse;
int i;
// noise filter
a->noise = new double[2 * a->size * 2]; // (double *)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);
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;
// level change
a->ntup = (int)(a->tup * a->rate);
a->ntdown = (int)(a->tdown * a->rate);
a->cup = new double[a->ntup + 1]; // (double *)malloc0 ((a->ntup + 1) * sizeof(double));
a->cdown = new double[a->ntdown + 1]; //(double *)malloc0 ((a->ntdown + 1) * sizeof(double));
delta = PI / (double)a->ntup;
theta = 0.0;
for (i = 0; i <= a->ntup; i++)
{
a->cup[i] = 0.5 * (1.0 - cos(theta));
theta += delta;
}
delta = PI / (double)a->ntdown;
theta = 0.0;
for (i = 0; i <= a->ntdown; i++)
{
a->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;
}
void FMSQ::decalc_fmsq (FMSQ *a)
{
delete[] (a->cdown);
delete[] (a->cup);
FIRCORE::destroy_fircore (a->p);
delete[] (a->noise);
}
FMSQ* FMSQ::create_fmsq (
int run,
int size,
double* insig,
double* outsig,
double* 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 *a = new FMSQ;
a->run = run;
a->size = size;
a->insig = insig;
a->outsig = outsig;
a->trigger = trigger;
a->rate = (double)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;
}
void FMSQ::destroy_fmsq (FMSQ *a)
{
decalc_fmsq (a);
delete (a);
}
void FMSQ::flush_fmsq (FMSQ *a)
{
FIRCORE::flush_fircore (a->p);
a->avnoise = 100.0;
a->longnoise = 1.0;
a->state = 0;
a->ready = 0;
a->ramp = 0.0;
}
enum _fmsqstate
{
MUTED,
INCREASE,
UNMUTED,
TAIL,
DECREASE
};
void FMSQ::xfmsq (FMSQ *a)
{
if (a->run)
{
int i;
double noise, lnlimit;
FIRCORE::xfircore (a->p);
for (i = 0; i < a->size; i++)
{
noise = sqrt(a->noise[2 * i + 0] * a->noise[2 * i + 0] + a->noise[2 * i + 1] * a->noise[2 * i + 1]);
a->avnoise = a->avm * a->avnoise + a->onem_avm * noise;
a->longnoise = a->longavm * a->longnoise + a->onem_longavm * noise;
if (!a->ready) a->ramp += a->rstep;
if (a->ramp >= a->tdelay) a->ready = 1;
switch (a->state)
{
case MUTED:
if (a->avnoise < a->unmute_thresh && a->ready)
{
a->state = INCREASE;
a->count = a->ntup;
}
a->outsig[2 * i + 0] = 0.0;
a->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];
if (a->count-- == 0)
a->state = UNMUTED;
break;
case UNMUTED:
if (a->avnoise > a->tail_thresh)
{
a->state = TAIL;
if ((lnlimit = a->longnoise) > 1.0) lnlimit = 1.0;
a->count = (int)((a->min_tail + (a->max_tail - a->min_tail) * lnlimit) * a->rate);
}
a->outsig[2 * i + 0] = a->insig[2 * i + 0];
a->outsig[2 * i + 1] = a->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];
if (a->avnoise < a->unmute_thresh)
a->state = UNMUTED;
else if (a->count-- == 0)
{
a->state = DECREASE;
a->count = a->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];
if (a->count-- == 0)
a->state = MUTED;
break;
}
}
}
else if (a->insig != a->outsig)
memcpy (a->outsig, a->insig, a->size * sizeof (dcomplex));
}
void FMSQ::setBuffers_fmsq (FMSQ *a, double* in, double* out, double* trig)
{
a->insig = in;
a->outsig = out;
a->trigger = trig;
FIRCORE::setBuffers_fircore (a->p, a->trigger, a->noise);
}
void FMSQ::setSamplerate_fmsq (FMSQ *a, int rate)
{
decalc_fmsq (a);
a->rate = rate;
calc_fmsq (a);
}
void FMSQ::setSize_fmsq (FMSQ *a, int size)
{
decalc_fmsq (a);
a->size = size;
calc_fmsq (a);
}
/********************************************************************************************************
* *
* RXA Properties *
* *
********************************************************************************************************/
void FMSQ::SetFMSQRun (RXA& rxa, int run)
{
rxa.csDSP.lock();
rxa.fmsq.p->run = run;
rxa.csDSP.unlock();
}
void FMSQ::SetFMSQThreshold (RXA& rxa, double threshold)
{
rxa.csDSP.lock();
rxa.fmsq.p->tail_thresh = threshold;
rxa.fmsq.p->unmute_thresh = 0.9 * threshold;
rxa.csDSP.unlock();
}
void FMSQ::SetFMSQNC (RXA& rxa, int nc)
{
FMSQ *a;
double* impulse;
rxa.csDSP.lock();
a = rxa.fmsq.p;
if (a->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);
delete[] (impulse);
}
rxa.csDSP.unlock();
}
void FMSQ::SetFMSQMP (RXA& rxa, int mp)
{
FMSQ *a;
a = rxa.fmsq.p;
if (a->mp != mp)
{
a->mp = mp;
FIRCORE::setMp_fircore (a->p, a->mp);
}
}
} // namespace WDSP