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.
299 lines
8.1 KiB
299 lines
8.1 KiB
/* 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 "fircore.hpp" |
|
#include "eq.hpp" |
|
#include "fmsq.hpp" |
|
#include "RXA.hpp" |
|
|
|
namespace WDSP { |
|
|
|
void FMSQ::calc_fmsq (FMSQ *a) |
|
{ |
|
float 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); |
|
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 float[a->ntup + 1]; // (float *)malloc0 ((a->ntup + 1) * sizeof(float)); |
|
a->cdown = new float[a->ntdown + 1]; //(float *)malloc0 ((a->ntdown + 1) * sizeof(float)); |
|
delta = PI / (float)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 / (float)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, |
|
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 *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; |
|
} |
|
|
|
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++) |
|
{ |
|
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; |
|
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) |
|
std::copy(a->insig, a->insig + a->size * 2, a->outsig); |
|
} |
|
|
|
void FMSQ::setBuffers_fmsq (FMSQ *a, float* in, float* out, float* 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.fmsq.p->run = run; |
|
} |
|
|
|
void FMSQ::SetFMSQThreshold (RXA& rxa, double threshold) |
|
{ |
|
rxa.fmsq.p->tail_thresh = threshold; |
|
rxa.fmsq.p->unmute_thresh = 0.9 * threshold; |
|
} |
|
|
|
void FMSQ::SetFMSQNC (RXA& rxa, int nc) |
|
{ |
|
FMSQ *a; |
|
float* impulse; |
|
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); |
|
} |
|
} |
|
|
|
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
|
|
|