kopia lustrzana https://github.com/f4exb/sdrangel
261 wiersze
7.4 KiB
C++
261 wiersze
7.4 KiB
C++
/* speak.cpp
|
|
|
|
This file is part of a program that implements a Software-Defined Radio.
|
|
|
|
Copyright (C) 2014, 2022, 2023 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 "speak.hpp"
|
|
#include "RXA.hpp"
|
|
#include "TXA.hpp"
|
|
|
|
namespace WDSP {
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* Complex Bi-Quad Peaking *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
void SPEAK::calc_speak (SPEAK *a)
|
|
{
|
|
double ratio;
|
|
double f_corr, g_corr, bw_corr, bw_parm, A, f_min;
|
|
|
|
switch (a->design)
|
|
{
|
|
case 0:
|
|
ratio = a->bw / a->f;
|
|
switch (a->nstages)
|
|
{
|
|
case 4:
|
|
bw_parm = 2.4;
|
|
f_corr = 1.0 - 0.160 * ratio + 1.440 * ratio * ratio;
|
|
g_corr = 1.0 - 1.003 * ratio + 3.990 * ratio * ratio;
|
|
break;
|
|
default:
|
|
bw_parm = 1.0;
|
|
f_corr = 1.0;
|
|
g_corr = 1.0;
|
|
break;
|
|
}
|
|
{
|
|
double fn, qk, qr, csn;
|
|
a->fgain = a->gain / g_corr;
|
|
fn = a->f / (double)a->rate / f_corr;
|
|
csn = cos (TWOPI * fn);
|
|
qr = 1.0 - 3.0 * a->bw / (double)a->rate * bw_parm;
|
|
qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn));
|
|
a->a0 = 1.0 - qk;
|
|
a->a1 = 2.0 * (qk - qr) * csn;
|
|
a->a2 = qr * qr - qk;
|
|
a->b1 = 2.0 * qr * csn;
|
|
a->b2 = - qr * qr;
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
if (a->f < 200.0) a->f = 200.0;
|
|
ratio = a->bw / a->f;
|
|
switch (a->nstages)
|
|
{
|
|
case 4:
|
|
bw_parm = 5.0;
|
|
bw_corr = 1.13 * ratio - 0.956 * ratio * ratio;
|
|
A = 2.5;
|
|
f_min = 50.0;
|
|
break;
|
|
default:
|
|
bw_parm = 1.0;
|
|
bw_corr = 1.0;
|
|
g_corr = 1.0;
|
|
A = 2.5;
|
|
f_min = 50.0;
|
|
break;
|
|
}
|
|
{
|
|
double w0, sn, c, den;
|
|
if (a->f < f_min) a->f = f_min;
|
|
w0 = TWOPI * a->f / (double)a->rate;
|
|
sn = sin (w0);
|
|
a->cbw = bw_corr * a->f;
|
|
c = sn * sinh(0.5 * log((a->f + 0.5 * a->cbw * bw_parm) / (a->f - 0.5 * a->cbw * bw_parm)) * w0 / sn);
|
|
den = 1.0 + c / A;
|
|
a->a0 = (1.0 + c * A) / den;
|
|
a->a1 = - 2.0 * cos (w0) / den;
|
|
a->a2 = (1 - c * A) / den;
|
|
a->b1 = - a->a1;
|
|
a->b2 = - (1 - c / A ) / den;
|
|
a->fgain = a->gain / pow (A * A, (double)a->nstages);
|
|
}
|
|
break;
|
|
}
|
|
flush_speak (a);
|
|
}
|
|
|
|
SPEAK* SPEAK::create_speak (
|
|
int run,
|
|
int size,
|
|
float* in,
|
|
float* out,
|
|
int rate,
|
|
double f,
|
|
double bw,
|
|
double gain,
|
|
int nstages,
|
|
int design
|
|
)
|
|
{
|
|
SPEAK *a = new SPEAK;
|
|
a->run = run;
|
|
a->size = size;
|
|
a->in = in;
|
|
a->out = out;
|
|
a->rate = rate;
|
|
a->f = f;
|
|
a->bw = bw;
|
|
a->gain = gain;
|
|
a->nstages = nstages;
|
|
a->design = design;
|
|
a->x0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex));
|
|
a->x1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex));
|
|
a->x2 = new double[a->nstages * 2]; //(float *) malloc0 (a->nstages * sizeof (complex));
|
|
a->y0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex));
|
|
a->y1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex));
|
|
a->y2 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex));
|
|
calc_speak (a);
|
|
return a;
|
|
}
|
|
|
|
void SPEAK::destroy_speak (SPEAK *a)
|
|
{
|
|
delete[] (a->y2);
|
|
delete[] (a->y1);
|
|
delete[] (a->y0);
|
|
delete[] (a->x2);
|
|
delete[] (a->x1);
|
|
delete[] (a->x0);
|
|
delete (a);
|
|
}
|
|
|
|
void SPEAK::flush_speak (SPEAK *a)
|
|
{
|
|
int i;
|
|
for (i = 0; i < a->nstages; i++)
|
|
{
|
|
a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0;
|
|
a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0;
|
|
}
|
|
}
|
|
|
|
void SPEAK::xspeak (SPEAK *a)
|
|
{
|
|
if (a->run)
|
|
{
|
|
int i, j, n;
|
|
|
|
for (i = 0; i < a->size; i++)
|
|
{
|
|
for (j = 0; j < 2; j++)
|
|
{
|
|
a->x0[j] = a->fgain * a->in[2 * i + j];
|
|
|
|
for (n = 0; n < a->nstages; n++)
|
|
{
|
|
if (n > 0)
|
|
a->x0[2 * n + j] = a->y0[2 * (n - 1) + j];
|
|
a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j]
|
|
+ a->a1 * a->x1[2 * n + j]
|
|
+ a->a2 * a->x2[2 * n + j]
|
|
+ a->b1 * a->y1[2 * n + j]
|
|
+ a->b2 * a->y2[2 * n + j];
|
|
a->y2[2 * n + j] = a->y1[2 * n + j];
|
|
a->y1[2 * n + j] = a->y0[2 * n + j];
|
|
a->x2[2 * n + j] = a->x1[2 * n + j];
|
|
a->x1[2 * n + j] = a->x0[2 * n + j];
|
|
}
|
|
|
|
a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j];
|
|
}
|
|
}
|
|
}
|
|
else if (a->out != a->in)
|
|
{
|
|
std::copy( a->in, a->in + a->size * 2, a->out);
|
|
}
|
|
}
|
|
|
|
void SPEAK::setBuffers_speak (SPEAK *a, float* in, float* out)
|
|
{
|
|
a->in = in;
|
|
a->out = out;
|
|
}
|
|
|
|
void SPEAK::setSamplerate_speak (SPEAK *a, int rate)
|
|
{
|
|
a->rate = rate;
|
|
calc_speak (a);
|
|
}
|
|
|
|
void SPEAK::setSize_speak (SPEAK *a, int size)
|
|
{
|
|
a->size = size;
|
|
flush_speak (a);
|
|
}
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* RXA Properties *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
void SPEAK::SetSPCWRun (RXA& rxa, int run)
|
|
{
|
|
SPEAK *a = rxa.speak;
|
|
a->run = run;
|
|
}
|
|
|
|
void SPEAK::SetSPCWFreq (RXA& rxa, double freq)
|
|
{
|
|
SPEAK *a = rxa.speak;
|
|
a->f = freq;
|
|
calc_speak (a);
|
|
}
|
|
|
|
void SPEAK::SetSPCWBandwidth (RXA& rxa, double bw)
|
|
{
|
|
SPEAK *a = rxa.speak;
|
|
a->bw = bw;
|
|
calc_speak (a);
|
|
}
|
|
|
|
void SPEAK::SetSPCWGain (RXA& rxa, double gain)
|
|
{
|
|
SPEAK *a = rxa.speak;
|
|
a->gain = gain;
|
|
calc_speak (a);
|
|
}
|
|
|
|
} // namespace WDSP
|