VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-14 12:22:00 -05:00
Code Issues Projects Releases Wiki Activity
98fb7a58d9
sdrangel/wdsp/snb.cpp

934 lines
27 KiB
C++
Raw Normal View History

Refactored WDSP initial commit
2024-06-16 05:31:13 -04:00
/* snb.c
This file is part of a program that implements a Software-Defined Radio.
Copyright (C) 2015, 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 "resample.hpp"
#include "lmath.hpp"
#include "firmin.hpp"
#include "nbp.hpp"
#include "amd.hpp"
#include "anf.hpp"
#include "anr.hpp"
#include "emnr.hpp"
#include "snb.hpp"
#include "RXA.hpp"
#define MAXIMP 256
namespace WDSP {
void SNBA::calc_snba (SNBA *d)
{
if (d->inrate >= d->internalrate)
d->isize = d->bsize / (d->inrate / d->internalrate);
else
d->isize = d->bsize * (d->internalrate / d->inrate);
d->inbuff = new double[d->isize * 2]; // (double *) malloc0 (d->isize * sizeof (complex));
d->outbuff = new double[d->isize * 2]; // (double *) malloc0 (d->isize * sizeof (complex));
if (d->inrate != d->internalrate)
d->resamprun = 1;
else
d->resamprun = 0;
d->inresamp = RESAMPLE::create_resample (d->resamprun, d->bsize, d->in, d->inbuff, d->inrate, d->internalrate, 0.0, 0, 2.0);
RESAMPLE::setFCLow_resample (d->inresamp, 250.0);
d->outresamp = RESAMPLE::create_resample (d->resamprun, d->isize, d->outbuff, d->out, d->internalrate, d->inrate, 0.0, 0, 2.0);
RESAMPLE::setFCLow_resample (d->outresamp, 200.0);
d->incr = d->xsize / d->ovrlp;
if (d->incr > d->isize)
d->iasize = d->incr;
else
d->iasize = d->isize;
d->iainidx = 0;
d->iaoutidx = 0;
d->inaccum = new double[d->isize]; // (double *) malloc0 (d->iasize * sizeof (double));
d->nsamps = 0;
if (d->incr > d->isize)
{
d->oasize = d->incr;
d->oainidx = 0;
d->oaoutidx = d->isize;
}
else
{
d->oasize = d->isize;
d->oainidx = 0;
d->oaoutidx = 0;
}
d->init_oaoutidx = d->oaoutidx;
d->outaccum = new double[d->oasize]; // (double *) malloc0 (d->oasize * sizeof (double));
}
SNBA* SNBA::create_snba (
int run,
double* in,
double* out,
int inrate,
int internalrate,
int bsize,
int ovrlp,
int xsize,
int asize,
int npasses,
double k1,
double k2,
int b,
int pre,
int post,
double pmultmin,
double out_low_cut,
double out_high_cut
)
{
SNBA *d = new SNBA;
d->run = run;
d->in = in;
d->out = out;
d->inrate = inrate;
d->internalrate = internalrate;
d->bsize = bsize;
d->ovrlp = ovrlp;
d->xsize = xsize;
d->exec.asize = asize;
d->exec.npasses = npasses;
d->sdet.k1 = k1;
d->sdet.k2 = k2;
d->sdet.b = b;
d->sdet.pre = pre;
d->sdet.post = post;
d->scan.pmultmin = pmultmin;
d->out_low_cut = out_low_cut;
d->out_high_cut = out_high_cut;
calc_snba (d);
d->xbase = new double[2 * d->xsize]; // (double *) malloc0 (2 * d->xsize * sizeof (double));
d->xaux = d->xbase + d->xsize;
d->exec.a = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->exec.v = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->exec.detout = new int[d->xsize]; // (int *) malloc0 (d->xsize * sizeof (int));
d->exec.savex = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->exec.xHout = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->exec.unfixed = new int32_t[d->xsize]; // (int *) malloc0 (d->xsize * sizeof (int));
d->sdet.vp = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->sdet.vpwr = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof (double));
d->wrk.xHat_a1rows_max = d->xsize + d->exec.asize;
d->wrk.xHat_a2cols_max = d->xsize + 2 * d->exec.asize;
d->wrk.xHat_r = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof(double));
d->wrk.xHat_ATAI = new double[d->xsize * d->xsize]; // (double *) malloc0 (d->xsize * d->xsize * sizeof(double));
d->wrk.xHat_A1 = new double[d->wrk.xHat_a1rows_max * d->xsize]; // (double *) malloc0 (d->wrk.xHat_a1rows_max * d->xsize * sizeof(double));
d->wrk.xHat_A2 = new double[d->wrk.xHat_a1rows_max * d->wrk.xHat_a2cols_max]; // (double *) malloc0 (d->wrk.xHat_a1rows_max * d->wrk.xHat_a2cols_max * sizeof(double));
d->wrk.xHat_P1 = new double[d->xsize * d->wrk.xHat_a2cols_max]; // (double *) malloc0 (d->xsize * d->wrk.xHat_a2cols_max * sizeof(double));
d->wrk.xHat_P2 = new double[d->xsize]; // (double *) malloc0 (d->xsize * sizeof(double));
d->wrk.trI_y = new double[d->xsize - 1]; // (double *) malloc0 ((d->xsize - 1) * sizeof(double));
d->wrk.trI_v = new double[d->xsize - 1]; // (double *) malloc0 ((d->xsize - 1) * sizeof(double));
d->wrk.dR_z = new double[d->xsize - 2]; // (double *) malloc0 ((d->xsize - 2) * sizeof(double));
d->wrk.asolve_r = new double[d->exec.asize + 1]; // (double *) malloc0 ((d->exec.asize + 1) * sizeof(double));
d->wrk.asolve_z = new double[d->exec.asize + 1]; // (double *) malloc0 ((d->exec.asize + 1) * sizeof(double));
return d;
}
void SNBA::decalc_snba (SNBA *d)
{
RESAMPLE::destroy_resample (d->outresamp);
RESAMPLE::destroy_resample (d->inresamp);
delete[] (d->outbuff);
delete[] (d->inbuff);
delete[] (d->outaccum);
delete[] (d->inaccum);
}
void SNBA::destroy_snba (SNBA *d)
{
delete[] (d->wrk.xHat_r);
delete[] (d->wrk.xHat_ATAI);
delete[] (d->wrk.xHat_A1);
delete[] (d->wrk.xHat_A2);
delete[] (d->wrk.xHat_P1);
delete[] (d->wrk.xHat_P2);
delete[] (d->wrk.trI_y);
delete[] (d->wrk.trI_v);
delete[] (d->wrk.dR_z);
delete[] (d->wrk.asolve_r);
delete[] (d->wrk.asolve_z);
delete[] (d->sdet.vpwr);
delete[] (d->sdet.vp);
delete[] (d->exec.unfixed);
delete[] (d->exec.xHout);
delete[] (d->exec.savex);
delete[] (d->exec.detout);
delete[] (d->exec.v);
delete[] (d->exec.a);
delete[] (d->xbase);
decalc_snba (d);
delete (d);
}
void SNBA::flush_snba (SNBA *d)
{
d->iainidx = 0;
d->iaoutidx = 0;
d->nsamps = 0;
d->oainidx = 0;
d->oaoutidx = d->init_oaoutidx;
memset (d->inaccum, 0, d->iasize * sizeof (double));
memset (d->outaccum, 0, d->oasize * sizeof (double));
memset (d->xaux, 0, d->xsize * sizeof (double));
memset (d->exec.a, 0, d->xsize * sizeof (double));
memset (d->exec.v, 0, d->xsize * sizeof (double));
memset (d->exec.detout, 0, d->xsize * sizeof (int));
memset (d->exec.savex, 0, d->xsize * sizeof (double));
memset (d->exec.xHout, 0, d->xsize * sizeof (double));
memset (d->exec.unfixed, 0, d->xsize * sizeof (int));
memset (d->sdet.vp, 0, d->xsize * sizeof (double));
memset (d->sdet.vpwr, 0, d->xsize * sizeof (double));
memset (d->inbuff, 0, d->isize * sizeof (dcomplex));
memset (d->outbuff, 0, d->isize * sizeof (dcomplex));
RESAMPLE::flush_resample (d->inresamp);
RESAMPLE::flush_resample (d->outresamp);
}
void SNBA::setBuffers_snba (SNBA *a, double* in, double* out)
{
decalc_snba (a);
a->in = in;
a->out = out;
calc_snba (a);
}
void SNBA::setSamplerate_snba (SNBA *a, int rate)
{
decalc_snba (a);
a->inrate = rate;
calc_snba (a);
}
void SNBA::setSize_snba (SNBA *a, int size)
{
decalc_snba (a);
a->bsize = size;
calc_snba (a);
}
void SNBA::ATAc0 (int n, int nr, double* A, double* r)
{
int i, j;
memset(r, 0, n * sizeof (double));
for (i = 0; i < n; i++)
for (j = 0; j < nr; j++)
r[i] += A[j * n + i] * A[j * n + 0];
}
void SNBA::multA1TA2(double* a1, double* a2, int m, int n, int q, double* c)
{
int i, j, k;
int p = q - m;
memset (c, 0, m * n * sizeof (double));
for (i = 0; i < m; i++)
{
for (j = 0; j < n; j++)
{
if (j < p)
{
for (k = i; k <= std::min(i + p, j); k++)
c[i * n + j] += a1[k * m + i] * a2[k * n + j];
}
if (j >= n - p)
{
for (k = std::max(i, q - (n - j)); k <= i + p; k++)
c[i * n + j] += a1[k * m + i] * a2[k * n + j];
}
}
}
}
void SNBA::multXKE(double* a, double* xk, int m, int q, int p, double* vout)
{
int i, k;
memset (vout, 0, m * sizeof (double));
for (i = 0; i < m; i++)
{
for (k = i; k < p; k++)
vout[i] += a[i * q + k] * xk[k];
for (k = q - p; k <= q - m + i; k++)
vout[i] += a[i * q + k] * xk[k];
}
}
void SNBA::multAv(double* a, double* v, int m, int q, double* vout)
{
int i, k;
memset (vout, 0, m * sizeof (double));
for (i = 0; i < m; i++)
{
for (k = 0; k < q; k++)
vout[i] += a[i * q + k] * v[k];
}
}
void SNBA::xHat(
int xusize,
int asize,
double* xk,
double* a,
double* xout,
double* r,
double* ATAI,
double* A1,
double* A2,
double* P1,
double* P2,
double* trI_y,
double* trI_v,
double* dR_z
)
{
int i, j, k;
int a1rows = xusize + asize;
int a2cols = xusize + 2 * asize;
memset (r, 0, xusize * sizeof(double)); // work space
memset (ATAI, 0, xusize * xusize * sizeof(double)); // work space
memset (A1, 0, a1rows * xusize * sizeof(double)); // work space
memset (A2, 0, a1rows * a2cols * sizeof(double)); // work space
memset (P1, 0, xusize * a2cols * sizeof(double)); // work space
memset (P2, 0, xusize * sizeof(double)); // work space
for (i = 0; i < xusize; i++)
{
A1[i * xusize + i] = 1.0;
k = i + 1;
for (j = k; j < k + asize; j++)
A1[j * xusize + i] = - a[j - k];
}
for (i = 0; i < asize; i++)
{
for (k = asize - i - 1, j = 0; k < asize; k++, j++)
A2[j * a2cols + i] = a[k];
}
for (i = asize + xusize; i < 2 * asize + xusize; i++)
{
A2[(i - asize) * a2cols + i] = - 1.0;
for (j = i - asize + 1, k = 0; j < xusize + asize; j++, k++)
A2[j * a2cols + i] = a[k];
}
ATAc0(xusize, xusize + asize, A1, r);
LMath::trI(xusize, r, ATAI, trI_y, trI_v, dR_z);
multA1TA2(A1, A2, xusize, 2 * asize + xusize, xusize + asize, P1);
multXKE(P1, xk, xusize, xusize + 2 * asize, asize, P2);
multAv(ATAI, P2, xusize, xusize, xout);
}
void SNBA::invf(int xsize, int asize, double* a, double* x, double* v)
{
int i, j;
memset (v, 0, xsize * sizeof (double));
for (i = asize; i < xsize - asize; i++)
{
for (j = 0; j < asize; j++)
v[i] += a[j] * (x[i - 1 - j] + x[i + 1 + j]);
v[i] = x[i] - 0.5 * v[i];
}
for (i = xsize - asize; i < xsize; i++)
{
for (j = 0; j < asize; j++)
v[i] += a[j] * x[i - 1 - j];
v[i] = x[i] - v[i];
}
}
void SNBA::det(SNBA *d, int asize, double* v, int* detout)
{
int i, j;
double medpwr, t1, t2;
int bstate, bcount, bsamp;
for (i = asize, j = 0; i < d->xsize; i++, j++)
{
d->sdet.vpwr[i] = v[i] * v[i];
d->sdet.vp[j] = d->sdet.vpwr[i];
}
LMath::median(d->xsize - asize, d->sdet.vp, &medpwr);
t1 = d->sdet.k1 * medpwr;
t2 = 0.0;
for (i = asize; i < d->xsize; i++)
{
if (d->sdet.vpwr[i] <= t1)
t2 += d->sdet.vpwr[i];
else if (d->sdet.vpwr[i] <= 2.0 * t1)
t2 += 2.0 * t1 - d->sdet.vpwr[i];
}
t2 *= d->sdet.k2 / (double)(d->xsize - asize);
for (i = asize; i < d->xsize; i++)
{
if (d->sdet.vpwr[i] > t2)
detout[i] = 1;
else
detout[i] = 0;
}
bstate = 0;
bcount = 0;
bsamp = 0;
for (i = asize; i < d->xsize; i++)
{
switch (bstate)
{
case 0:
if (detout[i] == 1) bstate = 1;
break;
case 1:
if (detout[i] == 0)
{
bstate = 2;
bsamp = i;
bcount = 1;
}
break;
case 2:
++bcount;
if (bcount > d->sdet.b)
if (detout[i] == 1)
bstate = 1;
else
bstate = 0;
else if (detout[i] == 1)
{
for (j = bsamp; j < bsamp + bcount - 1; j++)
detout[j] = 1;
bstate = 1;
}
break;
}
}
for (i = asize; i < d->xsize; i++)
{
if (detout[i] == 1)
{
for (j = i - 1; j > i - 1 - d->sdet.pre; j--)
if (j >= asize) detout[j] = 1;
}
}
for (i = d->xsize - 1; i >= asize; i--)
{
if (detout[i] == 1)
{
for (j = i + 1; j < i + 1 + d->sdet.post; j++)
if (j < d->xsize) detout[j] = 1;
}
}
}
int SNBA::scanFrame(
int xsize,
int pval,
double pmultmin,
int* det,
int* bimp,
int* limp,
int* befimp,
int* aftimp,
int* p_opt,
int* next
)
{
int inflag = 0;
int i = 0, j = 0, k = 0;
int nimp = 0;
double td;
int ti;
double merit[MAXIMP] = { 0 };
int nextlist[MAXIMP];
memset (befimp, 0, MAXIMP * sizeof (int));
memset (aftimp, 0, MAXIMP * sizeof (int));
while (i < xsize && nimp < MAXIMP)
{
if (det[i] == 1 && inflag == 0)
{
inflag = 1;
bimp[nimp] = i;
limp[nimp] = 1;
nimp++;
}
else if (det[i] == 1)
{
limp[nimp - 1]++;
}
else
{
inflag = 0;
befimp[nimp]++;
if (nimp > 0)
aftimp[nimp - 1]++;
}
i++;
}
for (i = 0; i < nimp; i++)
{
if (befimp[i] < aftimp[i])
p_opt[i] = befimp[i];
else
p_opt[i] = aftimp[i];
if (p_opt[i] > pval)
p_opt[i] = pval;
if (p_opt[i] < (int)(pmultmin * limp[i]))
p_opt[i] = -1;
}
for (i = 0; i < nimp; i++)
{
merit[i] = (double)p_opt[i] / (double)limp[i];
nextlist[i] = i;
}
for (j = 0; j < nimp - 1; j++)
{
for (k = 0; k < nimp - j - 1; k++)
{
if (merit[k] < merit[k + 1])
{
td = merit[k];
ti = nextlist[k];
merit[k] = merit[k + 1];
nextlist[k] = nextlist[k + 1];
merit[k + 1] = td;
nextlist[k + 1] = ti;
}
}
}
i = 1;
if (nimp > 0)
while (merit[i] == merit[0] && i < nimp) i++;
for (j = 0; j < i - 1; j++)
{
for (k = 0; k < i - j - 1; k++)
{
if (limp[nextlist[k]] < limp[nextlist[k + 1]])
{
td = merit[k];
ti = nextlist[k];
merit[k] = merit[k + 1];
nextlist[k] = nextlist[k + 1];
merit[k + 1] = td;
nextlist[k + 1] = ti;
}
}
}
*next = nextlist[0];
return nimp;
}
void SNBA::execFrame(SNBA *d, double* x)
{
int i, k;
int pass;
int nimp;
int bimp[MAXIMP];
int limp[MAXIMP];
int befimp[MAXIMP];
int aftimp[MAXIMP];
int p_opt[MAXIMP];
int next = 0;
int p;
memcpy (d->exec.savex, x, d->xsize * sizeof (double));
LMath::asolve(d->xsize, d->exec.asize, x, d->exec.a, d->wrk.asolve_r, d->wrk.asolve_z);
invf(d->xsize, d->exec.asize, d->exec.a, x, d->exec.v);
det(d, d->exec.asize, d->exec.v, d->exec.detout);
for (i = 0; i < d->xsize; i++)
{
if (d->exec.detout[i] != 0)
x[i] = 0.0;
}
nimp = scanFrame(d->xsize, d->exec.asize, d->scan.pmultmin, d->exec.detout, bimp, limp, befimp, aftimp, p_opt, &next);
for (pass = 0; pass < d->exec.npasses; pass++)
{
memcpy (d->exec.unfixed, d->exec.detout, d->xsize * sizeof (int));
for (k = 0; k < nimp; k++)
{
if (k > 0)
scanFrame(d->xsize, d->exec.asize, d->scan.pmultmin, d->exec.unfixed, bimp, limp, befimp, aftimp, p_opt, &next);
if ((p = p_opt[next]) > 0)
{
LMath::asolve(d->xsize, p, x, d->exec.a, d->wrk.asolve_r, d->wrk.asolve_z);
xHat(limp[next], p, &x[bimp[next] - p], d->exec.a, d->exec.xHout,
d->wrk.xHat_r, d->wrk.xHat_ATAI, d->wrk.xHat_A1, d->wrk.xHat_A2,
d->wrk.xHat_P1, d->wrk.xHat_P2, d->wrk.trI_y, d->wrk.trI_v, d->wrk.dR_z);
memcpy (&x[bimp[next]], d->exec.xHout, limp[next] * sizeof (double));
memset (&d->exec.unfixed[bimp[next]], 0, limp[next] * sizeof (int));
}
else
{
memcpy (&x[bimp[next]], &d->exec.savex[bimp[next]], limp[next] * sizeof (double));
}
}
}
}
void SNBA::xsnba (SNBA *d)
{
if (d->run)
{
int i;
RESAMPLE::xresample (d->inresamp);
for (i = 0; i < 2 * d->isize; i += 2)
{
d->inaccum[d->iainidx] = d->inbuff[i];
d->iainidx = (d->iainidx + 1) % d->iasize;
}
d->nsamps += d->isize;
while (d->nsamps >= d->incr)
{
memcpy (&d->xaux[d->xsize - d->incr], &d->inaccum[d->iaoutidx], d->incr * sizeof (double));
execFrame (d, d->xaux);
d->iaoutidx = (d->iaoutidx + d->incr) % d->iasize;
d->nsamps -= d->incr;
memcpy (&d->outaccum[d->oainidx], d->xaux, d->incr * sizeof (double));
d->oainidx = (d->oainidx + d->incr) % d->oasize;
memmove (d->xbase, &d->xbase[d->incr], (2 * d->xsize - d->incr) * sizeof (double));
}
for (i = 0; i < d->isize; i++)
{
d->outbuff[2 * i + 0] = d->outaccum[d->oaoutidx];
d->outbuff[2 * i + 1] = 0.0;
d->oaoutidx = (d->oaoutidx + 1) % d->oasize;
}
RESAMPLE::xresample (d->outresamp);
}
else if (d->out != d->in)
memcpy (d->out, d->in, d->bsize * sizeof (dcomplex));
}
/********************************************************************************************************
* *
* RXA Properties *
* *
********************************************************************************************************/
void SNBA::SetSNBARun (RXA& rxa, int run)
{
SNBA *a = rxa.snba.p;
if (a->run != run)
{
BPSNBA::bpsnbaCheck (rxa, rxa.mode, rxa.ndb.p->master_run);
RXA::bp1Check (rxa, rxa.amd.p->run, run, rxa.emnr.p->run,
rxa.anf.p->run, rxa.anr.p->run);
rxa.csDSP.lock();
a->run = run;
RXA::bp1Set (rxa);
BPSNBA::bpsnbaSet (rxa);
rxa.csDSP.unlock();
}
}
void SNBA::SetSNBAovrlp (RXA& rxa, int ovrlp)
{
rxa.csDSP.lock();
decalc_snba (rxa.snba.p);
rxa.snba.p->ovrlp = ovrlp;
calc_snba (rxa.snba.p);
rxa.csDSP.unlock();
}
void SetSNBAasize (RXA& rxa, int size)
{
rxa.csDSP.lock();
rxa.snba.p->exec.asize = size;
rxa.csDSP.unlock();
}
void SNBA::SetSNBAnpasses (RXA& rxa, int npasses)
{
rxa.csDSP.lock();
rxa.snba.p->exec.npasses = npasses;
rxa.csDSP.unlock();
}
void SNBA::SetSNBAk1 (RXA& rxa, double k1)
{
rxa.csDSP.lock();
rxa.snba.p->sdet.k1 = k1;
rxa.csDSP.unlock();
}
void SNBA::SetSNBAk2 (RXA& rxa, double k2)
{
rxa.csDSP.lock();
rxa.snba.p->sdet.k2 = k2;
rxa.csDSP.unlock();
}
void SNBA::SetSNBAbridge (RXA& rxa, int bridge)
{
rxa.csDSP.lock();
rxa.snba.p->sdet.b = bridge;
rxa.csDSP.unlock();
}
void SNBA::SetSNBApresamps (RXA& rxa, int presamps)
{
rxa.csDSP.lock();
rxa.snba.p->sdet.pre = presamps;
rxa.csDSP.unlock();
}
void SNBA::SetSNBApostsamps (RXA& rxa, int postsamps)
{
rxa.csDSP.lock();
rxa.snba.p->sdet.post = postsamps;
rxa.csDSP.unlock();
}
void SNBA::SetSNBApmultmin (RXA& rxa, double pmultmin)
{
rxa.csDSP.lock();
rxa.snba.p->scan.pmultmin = pmultmin;
rxa.csDSP.unlock();
}
void SNBA::SetSNBAOutputBandwidth (RXA& rxa, double flow, double fhigh)
{
SNBA *a;
RESAMPLE *d;
double f_low, f_high;
rxa.csDSP.lock();
a = rxa.snba.p;
d = a->outresamp;
if (flow >= 0 && fhigh >= 0)
{
if (fhigh < a->out_low_cut) fhigh = a->out_low_cut;
if (flow > a->out_high_cut) flow = a->out_high_cut;
f_low = std::max ( a->out_low_cut, flow);
f_high = std::min (a->out_high_cut, fhigh);
}
else if (flow <= 0 && fhigh <= 0)
{
if (flow > -a->out_low_cut) flow = -a->out_low_cut;
if (fhigh < -a->out_high_cut) fhigh = -a->out_high_cut;
f_low = std::max ( a->out_low_cut, -fhigh);
f_high = std::min (a->out_high_cut, -flow);
}
else if (flow < 0 && fhigh > 0)
{
double absmax = std::max (-flow, fhigh);
if (absmax < a->out_low_cut) absmax = a->out_low_cut;
f_low = a->out_low_cut;
f_high = std::min (a->out_high_cut, absmax);
}
RESAMPLE::setBandwidth_resample (d, f_low, f_high);
rxa.csDSP.unlock();
}
/********************************************************************************************************
* *
* BPSNBA Bandpass Filter *
* *
********************************************************************************************************/
// This is a thin wrapper for a notched-bandpass filter (nbp). The basic difference is that it provides
// for its input and output to happen at different points in the processing pipeline. This means it must
// include a buffer, 'buff'. Its input and output are done via functions xbpshbain() and xbpshbaout().
void BPSNBA::calc_bpsnba (BPSNBA *a)
{
a->buff = new double[a->size * 2]; // (double *) malloc0 (a->size * sizeof (complex));
a->bpsnba = NBP::create_nbp (
1, // run, always runs (use bpsnba 'run')
a->run_notches, // run the notches
0, // position variable for nbp (not for bpsnba), always 0
a->size, // buffer size
a->nc, // number of filter coefficients
a->mp, // minimum phase flag
a->buff, // pointer to input buffer
a->out, // pointer to output buffer
a->f_low, // lower filter frequency
a->f_high, // upper filter frequency
a->rate, // sample rate
a->wintype, // wintype
a->gain, // gain
a->autoincr, // auto-increase notch width if below min
a->maxpb, // max number of passbands
a->ptraddr); // addr of database pointer
}
BPSNBA* BPSNBA::create_bpsnba (
int run,
int run_notches,
int position,
int size,
int nc,
int mp,
double* in,
double* out,
int rate,
double abs_low_freq,
double abs_high_freq,
double f_low,
double f_high,
int wintype,
double gain,
int autoincr,
int maxpb,
NOTCHDB* ptraddr
)
{
BPSNBA *a = new BPSNBA;
a->run = run;
a->run_notches = run_notches;
a->position = position;
a->size = size;
a->nc = nc;
a->mp = mp;
a->in = in;
a->out = out;
a->rate = rate;
a->abs_low_freq = abs_low_freq;
a->abs_high_freq = abs_high_freq;
a->f_low = f_low;
a->f_high = f_high;
a->wintype = wintype;
a->gain = gain;
a->autoincr = autoincr;
a->maxpb = maxpb;
a->ptraddr = ptraddr;
calc_bpsnba (a);
return a;
}
void BPSNBA::decalc_bpsnba (BPSNBA *a)
{
NBP::destroy_nbp (a->bpsnba);
delete[] (a->buff);
}
void BPSNBA::destroy_bpsnba (BPSNBA *a)
{
decalc_bpsnba (a);
delete[] (a);
}
void BPSNBA::flush_bpsnba (BPSNBA *a)
{
memset (a->buff, 0, a->size * sizeof (dcomplex));
NBP::flush_nbp (a->bpsnba);
}
void BPSNBA::setBuffers_bpsnba (BPSNBA *a, double* in, double* out)
{
decalc_bpsnba (a);
a->in = in;
a->out = out;
calc_bpsnba (a);
}
void BPSNBA::setSamplerate_bpsnba (BPSNBA *a, int rate)
{
decalc_bpsnba (a);
a->rate = rate;
calc_bpsnba (a);
}
void BPSNBA::setSize_bpsnba (BPSNBA *a, int size)
{
decalc_bpsnba (a);
a->size = size;
calc_bpsnba (a);
}
void BPSNBA::xbpsnbain (BPSNBA *a, int position)
{
if (a->run && a->position == position)
memcpy (a->buff, a->in, a->size * sizeof (dcomplex));
}
void BPSNBA::xbpsnbaout (BPSNBA *a, int position)
{
if (a->run && a->position == position)
NBP::xnbp (a->bpsnba, 0);
}
void BPSNBA::recalc_bpsnba_filter (BPSNBA *a, int update)
{
// Call anytime one of the parameters listed below has been changed in
// the BPSNBA struct.
NBP *b = a->bpsnba;
b->fnfrun = a->run_notches;
b->flow = a->f_low;
b->fhigh = a->f_high;
b->wintype = a->wintype;
b->gain = a->gain;
b->autoincr = a->autoincr;
NBP::calc_nbp_impulse (b);
FIRCORE::setImpulse_fircore (b->p, b->impulse, update);
delete[] (b->impulse);
}
/********************************************************************************************************
* *
* RXA Properties *
* *
********************************************************************************************************/
void BPSNBA::BPSNBASetNC (RXA& rxa, int nc)
{
BPSNBA *a;
rxa.csDSP.lock();
a = rxa.bpsnba.p;
if (a->nc != nc)
{
a->nc = nc;
a->bpsnba->nc = a->nc;
NBP::setNc_nbp (a->bpsnba);
}
rxa.csDSP.unlock();
}
void BPSNBA::BPSNBASetMP (RXA& rxa, int mp)
{
BPSNBA *a;
a = rxa.bpsnba.p;
if (a->mp != mp)
{
a->mp = mp;
a->bpsnba->mp = a->mp;
NBP::setMp_nbp (a->bpsnba);
}
}
} // namespace WDSP
Reference in New Issue Copy Permalink