623 lines
14 KiB
C
623 lines
14 KiB
C
/* nbp.c
|
|
|
|
This file is part of a program that implements a Software-Defined Radio.
|
|
|
|
Copyright (C) 2015, 2016 Warren Pratt, NR0V
|
|
|
|
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.h"
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* Notch Database *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
NOTCHDB create_notchdb (int master_run, int maxnotches)
|
|
{
|
|
NOTCHDB a = (NOTCHDB) malloc0 (sizeof (notchdb));
|
|
a->master_run = master_run;
|
|
a->maxnotches = maxnotches;
|
|
a->nn = 0;
|
|
a->fcenter = (double *) malloc0 (a->maxnotches * sizeof (double));
|
|
a->fwidth = (double *) malloc0 (a->maxnotches * sizeof (double));
|
|
a->nlow = (double *) malloc0 (a->maxnotches * sizeof (double));
|
|
a->nhigh = (double *) malloc0 (a->maxnotches * sizeof (double));
|
|
a->active = (int *) malloc0 (a->maxnotches * sizeof (int ));
|
|
return a;
|
|
}
|
|
|
|
void destroy_notchdb (NOTCHDB b)
|
|
{
|
|
_aligned_free (b->active);
|
|
_aligned_free (b->nhigh);
|
|
_aligned_free (b->nlow);
|
|
_aligned_free (b->fwidth);
|
|
_aligned_free (b->fcenter);
|
|
}
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* Notched Bandpass Filter *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
double* fir_mbandpass (int N, int nbp, double* flow, double* fhigh, double rate, double scale, int wintype)
|
|
{
|
|
int i, k;
|
|
double* impulse = (double *) malloc0 (N * sizeof (complex));
|
|
double* imp;
|
|
for (k = 0; k < nbp; k++)
|
|
{
|
|
imp = fir_bandpass (N, flow[k], fhigh[k], rate, wintype, 1, scale);
|
|
for (i = 0; i < N; i++)
|
|
{
|
|
impulse[2 * i + 0] += imp[2 * i + 0];
|
|
impulse[2 * i + 1] += imp[2 * i + 1];
|
|
}
|
|
_aligned_free (imp);
|
|
}
|
|
return impulse;
|
|
}
|
|
|
|
double min_notch_width (NBP a)
|
|
{
|
|
double min_width;
|
|
switch (a->wintype)
|
|
{
|
|
case 0:
|
|
min_width = 1600.0 / (a->nc / 256) * (a->rate / 48000);
|
|
break;
|
|
case 1:
|
|
min_width = 2200.0 / (a->nc / 256) * (a->rate / 48000);
|
|
break;
|
|
default:
|
|
min_width = 2200.0 / (a->nc / 256) * (a->rate / 48000);
|
|
break;
|
|
}
|
|
return min_width;
|
|
}
|
|
|
|
int make_nbp (int nn, int* active, double* center, double* width, double* nlow, double* nhigh,
|
|
double minwidth, int autoincr, double flow, double fhigh, double* bplow, double* bphigh, int* havnotch)
|
|
{
|
|
int nbp;
|
|
int nnbp, adds;
|
|
int i, j, k;
|
|
double nl, nh;
|
|
int* del = (int *) malloc0 (1024 * sizeof (int));
|
|
if (fhigh > flow)
|
|
{
|
|
bplow[0] = flow;
|
|
bphigh[0] = fhigh;
|
|
nbp = 1;
|
|
}
|
|
else
|
|
{
|
|
nbp = 0;
|
|
return nbp;
|
|
}
|
|
*havnotch = 0;
|
|
for (k = 0; k < nn; k++)
|
|
{
|
|
if (autoincr && width[k] < minwidth)
|
|
{
|
|
nl = center[k] - 0.5 * minwidth;
|
|
nh = center[k] + 0.5 * minwidth;
|
|
}
|
|
else
|
|
{
|
|
nl = nlow[k];
|
|
nh = nhigh[k];
|
|
}
|
|
if (active[k] && (nh > flow && nl < fhigh))
|
|
{
|
|
*havnotch = 1;
|
|
adds = 0;
|
|
for (i = 0; i < nbp; i++)
|
|
{
|
|
if (nh > bplow[i] && nl < bphigh[i])
|
|
{
|
|
if (nl <= bplow[i] && nh >= bphigh[i])
|
|
{
|
|
del[i] = 1;
|
|
}
|
|
else if (nl > bplow[i] && nh < bphigh[i])
|
|
{
|
|
|
|
bplow[nbp + adds] = nh;
|
|
bphigh[nbp + adds] = bphigh[i];
|
|
bphigh[i] = nl;
|
|
adds++;
|
|
}
|
|
else if (nl <= bplow[i] && nh > bplow[i])
|
|
{
|
|
bplow[i] = nh;
|
|
}
|
|
else if (nl < bphigh[i] && nh >= bphigh[i])
|
|
{
|
|
bphigh[i] = nl;
|
|
}
|
|
}
|
|
}
|
|
nbp += adds;
|
|
nnbp = nbp;
|
|
for (i = 0; i < nbp; i++)
|
|
{
|
|
if (del[i] == 1)
|
|
{
|
|
nnbp--;
|
|
for (j = i; j < nnbp; j++)
|
|
{
|
|
bplow[j] = bplow[j + 1];
|
|
bphigh[j] = bphigh[j + 1];
|
|
}
|
|
del[i] = 0;
|
|
}
|
|
}
|
|
nbp = nnbp;
|
|
}
|
|
}
|
|
_aligned_free (del);
|
|
return nbp;
|
|
}
|
|
|
|
void calc_nbp_lightweight (NBP a)
|
|
{ // calculate and set new impulse response; used when changing tune freq or shift freq
|
|
int i;
|
|
double fl, fh;
|
|
double offset;
|
|
NOTCHDB b = *a->ptraddr;
|
|
if (a->fnfrun)
|
|
{
|
|
offset = b->tunefreq + b->shift;
|
|
fl = a->flow + offset;
|
|
fh = a->fhigh + offset;
|
|
a->numpb = make_nbp (b->nn, b->active, b->fcenter, b->fwidth, b->nlow, b->nhigh,
|
|
min_notch_width (a), a->autoincr, fl, fh, a->bplow, a->bphigh, &a->havnotch);
|
|
// when tuning, no need to recalc filter if there were not and are not any notches in passband
|
|
if (a->hadnotch || a->havnotch)
|
|
{
|
|
for (i = 0; i < a->numpb; i++)
|
|
{
|
|
a->bplow[i] -= offset;
|
|
a->bphigh[i] -= offset;
|
|
}
|
|
a->impulse = fir_mbandpass (a->nc, a->numpb, a->bplow, a->bphigh,
|
|
a->rate, a->gain / (double)(2 * a->size), a->wintype);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
// print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
|
|
_aligned_free(a->impulse);
|
|
}
|
|
a->hadnotch = a->havnotch;
|
|
}
|
|
else
|
|
a->hadnotch = 1;
|
|
}
|
|
|
|
void calc_nbp_impulse (NBP a)
|
|
{ // calculates impulse response; for create_fircore() and parameter changes
|
|
int i;
|
|
double fl, fh;
|
|
double offset;
|
|
NOTCHDB b = *a->ptraddr;
|
|
if (a->fnfrun)
|
|
{
|
|
offset = b->tunefreq + b->shift;
|
|
fl = a->flow + offset;
|
|
fh = a->fhigh + offset;
|
|
a->numpb = make_nbp (b->nn, b->active, b->fcenter, b->fwidth, b->nlow, b->nhigh,
|
|
min_notch_width (a), a->autoincr, fl, fh, a->bplow, a->bphigh, &a->havnotch);
|
|
for (i = 0; i < a->numpb; i++)
|
|
{
|
|
a->bplow[i] -= offset;
|
|
a->bphigh[i] -= offset;
|
|
}
|
|
a->impulse = fir_mbandpass (a->nc, a->numpb, a->bplow, a->bphigh,
|
|
a->rate, a->gain / (double)(2 * a->size), a->wintype);
|
|
}
|
|
else
|
|
{
|
|
a->impulse = fir_bandpass(a->nc, a->flow, a->fhigh, a->rate, a->wintype, 1, a->gain / (double)(2 * a->size));
|
|
}
|
|
}
|
|
|
|
NBP create_nbp(int run, int fnfrun, int position, int size, int nc, int mp, double* in, double* out,
|
|
double flow, double fhigh, int rate, int wintype, double gain, int autoincr, int maxpb, NOTCHDB* ptraddr)
|
|
{
|
|
NBP a = (NBP) malloc0 (sizeof (nbp));
|
|
a->run = run;
|
|
a->fnfrun = fnfrun;
|
|
a->position = position;
|
|
a->size = size;
|
|
a->nc = nc;
|
|
a->mp = mp;
|
|
a->rate = (double)rate;
|
|
a->wintype = wintype;
|
|
a->gain = gain;
|
|
a->in = in;
|
|
a->out = out;
|
|
a->autoincr = autoincr;
|
|
a->flow = flow;
|
|
a->fhigh = fhigh;
|
|
a->maxpb = maxpb;
|
|
a->ptraddr = ptraddr;
|
|
a->bplow = (double *) malloc0 (a->maxpb * sizeof (double));
|
|
a->bphigh = (double *) malloc0 (a->maxpb * sizeof (double));
|
|
calc_nbp_impulse (a);
|
|
a->p = create_fircore (a->size, a->in, a->out, a->nc, a->mp, a->impulse);
|
|
// print_impulse ("nbp.txt", a->size + 1, impulse, 1, 0);
|
|
_aligned_free(a->impulse);
|
|
return a;
|
|
}
|
|
|
|
void destroy_nbp (NBP a)
|
|
{
|
|
destroy_fircore (a->p);
|
|
_aligned_free (a->bphigh);
|
|
_aligned_free (a->bplow);
|
|
_aligned_free (a);
|
|
}
|
|
|
|
void flush_nbp (NBP a)
|
|
{
|
|
flush_fircore (a->p);
|
|
}
|
|
|
|
void xnbp (NBP a, int pos)
|
|
{
|
|
if (a->run && pos == a->position)
|
|
xfircore (a->p);
|
|
else if (a->in != a->out)
|
|
memcpy (a->out, a->in, a->size * sizeof (complex));
|
|
}
|
|
|
|
void setBuffers_nbp (NBP a, double* in, double* out)
|
|
{
|
|
a->in = in;
|
|
a->out = out;
|
|
setBuffers_fircore (a->p, a->in, a->out);
|
|
}
|
|
|
|
void setSamplerate_nbp (NBP a, int rate)
|
|
{
|
|
a->rate = rate;
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
|
|
void setSize_nbp (NBP a, int size)
|
|
{
|
|
// NOTE: 'size' must be <= 'nc'
|
|
a->size = size;
|
|
setSize_fircore (a->p, a->size);
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
|
|
void setNc_nbp (NBP a)
|
|
{
|
|
calc_nbp_impulse (a);
|
|
setNc_fircore (a->p, a->nc, a->impulse);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
|
|
void setMp_nbp (NBP a)
|
|
{
|
|
setMp_fircore (a->p, a->mp);
|
|
}
|
|
|
|
/********************************************************************************************************
|
|
* *
|
|
* RXA Properties *
|
|
* *
|
|
********************************************************************************************************/
|
|
|
|
// DATABASE PROPERTIES
|
|
|
|
void UpdateNBPFiltersLightWeight (int channel)
|
|
{ // called when setting tune freq or shift freq
|
|
calc_nbp_lightweight (rxa[channel].nbp0.p);
|
|
calc_nbp_lightweight (rxa[channel].bpsnba.p->bpsnba);
|
|
}
|
|
|
|
void UpdateNBPFilters(int channel)
|
|
{
|
|
NBP a = rxa[channel].nbp0.p;
|
|
BPSNBA b = rxa[channel].bpsnba.p;
|
|
if (a->fnfrun)
|
|
{
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
if (b->bpsnba->fnfrun)
|
|
{
|
|
recalc_bpsnba_filter (b, 1);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
int RXANBPAddNotch (int channel, int notch, double fcenter, double fwidth, int active)
|
|
{
|
|
NOTCHDB b;
|
|
int i, j;
|
|
int rval;
|
|
b = rxa[channel].ndb.p;
|
|
if (notch <= b->nn && b->nn < b->maxnotches)
|
|
{
|
|
b->nn++;
|
|
for (i = b->nn - 2, j = b->nn - 1; i >= notch; i--, j--)
|
|
{
|
|
b->fcenter[j] = b->fcenter[i];
|
|
b->fwidth[j] = b->fwidth[i];
|
|
b->nlow[j] = b->nlow[i];
|
|
b->nhigh[j] = b->nhigh[i];
|
|
b->active[j] = b->active[i];
|
|
}
|
|
b->fcenter[notch] = fcenter;
|
|
b->fwidth[notch] = fwidth;
|
|
b->nlow[notch] = fcenter - 0.5 * fwidth;
|
|
b->nhigh[notch] = fcenter + 0.5 * fwidth;
|
|
b->active[notch] = active;
|
|
UpdateNBPFilters (channel);
|
|
rval = 0;
|
|
}
|
|
else
|
|
rval = -1;
|
|
return rval;
|
|
}
|
|
|
|
PORT
|
|
int RXANBPGetNotch (int channel, int notch, double* fcenter, double* fwidth, int* active)
|
|
{
|
|
NOTCHDB a;
|
|
int rval;
|
|
EnterCriticalSection (&ch[channel].csDSP);
|
|
a = rxa[channel].ndb.p;
|
|
if (notch < a->nn)
|
|
{
|
|
*fcenter = a->fcenter[notch];
|
|
*fwidth = a->fwidth[notch];
|
|
*active = a->active[notch];
|
|
rval = 0;
|
|
}
|
|
else
|
|
{
|
|
*fcenter = -1.0;
|
|
*fwidth = 0.0;
|
|
*active = -1;
|
|
rval = -1;
|
|
}
|
|
LeaveCriticalSection (&ch[channel].csDSP);
|
|
return rval;
|
|
}
|
|
|
|
PORT
|
|
int RXANBPDeleteNotch (int channel, int notch)
|
|
{
|
|
int i, j;
|
|
int rval;
|
|
NOTCHDB a;
|
|
a = rxa[channel].ndb.p;
|
|
if (notch < a->nn)
|
|
{
|
|
a->nn--;
|
|
for (i = notch, j = notch + 1; i < a->nn; i++, j++)
|
|
{
|
|
a->fcenter[i] = a->fcenter[j];
|
|
a->fwidth[i] = a->fwidth[j];
|
|
a->nlow[i] = a->nlow[j];
|
|
a->nhigh[i] = a->nhigh[j];
|
|
a->active[i] = a->active[j];
|
|
}
|
|
UpdateNBPFilters (channel);
|
|
rval = 0;
|
|
}
|
|
else
|
|
rval = -1;
|
|
return rval;
|
|
}
|
|
|
|
PORT
|
|
int RXANBPEditNotch (int channel, int notch, double fcenter, double fwidth, int active)
|
|
{
|
|
NOTCHDB a;
|
|
int rval;
|
|
a = rxa[channel].ndb.p;
|
|
if (notch < a->nn)
|
|
{
|
|
a->fcenter[notch] = fcenter;
|
|
a->fwidth[notch] = fwidth;
|
|
a->nlow[notch] = fcenter - 0.5 * fwidth;
|
|
a->nhigh[notch] = fcenter + 0.5 * fwidth;
|
|
a->active[notch] = active;
|
|
UpdateNBPFilters (channel);
|
|
rval = 0;
|
|
}
|
|
else
|
|
rval = -1;
|
|
return rval;
|
|
}
|
|
|
|
PORT
|
|
void RXANBPGetNumNotches (int channel, int* nnotches)
|
|
{
|
|
NOTCHDB a;
|
|
EnterCriticalSection (&ch[channel].csDSP);
|
|
a = rxa[channel].ndb.p;
|
|
*nnotches = a->nn;
|
|
LeaveCriticalSection (&ch[channel].csDSP);
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetTuneFrequency (int channel, double tunefreq)
|
|
{
|
|
NOTCHDB a;
|
|
a = rxa[channel].ndb.p;
|
|
if (tunefreq != a->tunefreq)
|
|
{
|
|
a->tunefreq = tunefreq;
|
|
UpdateNBPFiltersLightWeight (channel);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetShiftFrequency (int channel, double shift)
|
|
{
|
|
NOTCHDB a;
|
|
a = rxa[channel].ndb.p;
|
|
if (shift != a->shift)
|
|
{
|
|
a->shift = shift;
|
|
UpdateNBPFiltersLightWeight (channel);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetNotchesRun (int channel, int run)
|
|
{
|
|
NOTCHDB a = rxa[channel].ndb.p;
|
|
NBP b = rxa[channel].nbp0.p;
|
|
if ( run != a->master_run)
|
|
{
|
|
a->master_run = run; // update variables
|
|
b->fnfrun = a->master_run;
|
|
RXAbpsnbaCheck (channel, rxa[channel].mode, run);
|
|
calc_nbp_impulse (b); // recalc nbp impulse response
|
|
setImpulse_fircore (b->p, b->impulse, 0); // calculate new filter masks
|
|
_aligned_free (b->impulse);
|
|
EnterCriticalSection (&ch[channel].csDSP); // block DSP channel processing
|
|
RXAbpsnbaSet (channel);
|
|
setUpdate_fircore (b->p); // apply new filter masks
|
|
LeaveCriticalSection (&ch[channel].csDSP); // unblock channel processing
|
|
}
|
|
}
|
|
|
|
// FILTER PROPERTIES
|
|
|
|
PORT
|
|
void RXANBPSetRun (int channel, int run)
|
|
{
|
|
NBP a;
|
|
EnterCriticalSection (&ch[channel].csDSP);
|
|
a = rxa[channel].nbp0.p;
|
|
a->run = run;
|
|
LeaveCriticalSection (&ch[channel].csDSP);
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetFreqs (int channel, double flow, double fhigh)
|
|
{
|
|
NBP a;
|
|
a = rxa[channel].nbp0.p;
|
|
if ((flow != a->flow) || (fhigh != a->fhigh))
|
|
{
|
|
a->flow = flow;
|
|
a->fhigh = fhigh;
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetWindow (int channel, int wintype)
|
|
{
|
|
NBP a;
|
|
BPSNBA b;
|
|
a = rxa[channel].nbp0.p;
|
|
b = rxa[channel].bpsnba.p;
|
|
if ((a->wintype != wintype))
|
|
{
|
|
a->wintype = wintype;
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
if ((b->wintype != wintype))
|
|
{
|
|
b->wintype = wintype;
|
|
recalc_bpsnba_filter (b, 1);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetNC (int channel, int nc)
|
|
{
|
|
// NOTE: 'nc' must be >= 'size'
|
|
NBP a;
|
|
EnterCriticalSection (&ch[channel].csDSP);
|
|
a = rxa[channel].nbp0.p;
|
|
if (a->nc != nc)
|
|
{
|
|
a->nc = nc;
|
|
setNc_nbp (a);
|
|
}
|
|
LeaveCriticalSection (&ch[channel].csDSP);
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetMP (int channel, int mp)
|
|
{
|
|
NBP a;
|
|
a = rxa[channel].nbp0.p;
|
|
if (a->mp != mp)
|
|
{
|
|
a->mp = mp;
|
|
setMp_nbp (a);
|
|
}
|
|
}
|
|
|
|
PORT
|
|
void RXANBPGetMinNotchWidth (int channel, double* minwidth)
|
|
{
|
|
NBP a;
|
|
EnterCriticalSection (&ch[channel].csDSP);
|
|
a = rxa[channel].nbp0.p;
|
|
*minwidth = min_notch_width (a);
|
|
LeaveCriticalSection (&ch[channel].csDSP);
|
|
}
|
|
|
|
PORT
|
|
void RXANBPSetAutoIncrease (int channel, int autoincr)
|
|
{
|
|
NBP a;
|
|
BPSNBA b;
|
|
a = rxa[channel].nbp0.p;
|
|
b = rxa[channel].bpsnba.p;
|
|
if ((a->autoincr != autoincr))
|
|
{
|
|
a->autoincr = autoincr;
|
|
calc_nbp_impulse (a);
|
|
setImpulse_fircore (a->p, a->impulse, 1);
|
|
_aligned_free (a->impulse);
|
|
}
|
|
if ((b->autoincr != autoincr))
|
|
{
|
|
b->autoincr = autoincr;
|
|
recalc_bpsnba_filter (b, 1);
|
|
}
|
|
}
|