wdsp/nobII.c
Uladzimir Karpenka 89c8a0e2b5 first commit
2026-06-01 15:58:45 +03:00

760 lines
19 KiB
C

/* nobII.c
This file is part of a program that implements a Software-Defined Radio.
Copyright (C) 2014 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"
#define MAX_ADV_SLEW_TIME (0.002)
#define MAX_ADV_TIME (0.002)
#define MAX_HANG_SLEW_TIME (0.002)
#define MAX_HANG_TIME (0.002)
#define MAX_SEQ_TIME (0.025)
#define MAX_SAMPLERATE (1536000.0)
void init_nob (NOB a)
{
int i;
double coef;
a->adv_slew_count = (int)(a->advslewtime * a->samplerate);
a->adv_count = (int)(a->advtime * a->samplerate);
a->hang_count = (int)(a->hangtime * a->samplerate);
a->hang_slew_count = (int)(a->hangslewtime * a->samplerate);
a->max_imp_seq = (int)(a->max_imp_seq_time * a->samplerate);
a->backmult = exp (-1.0 / (a->samplerate * a->backtau));
a->ombackmult = 1.0 - a->backmult;
if (a->adv_slew_count > 0)
{
coef = PI / (a->adv_slew_count + 1);
for (i = 0; i < a->adv_slew_count; i++)
a->awave[i] = 0.5 * cos ((i + 1) * coef);
}
if (a->hang_slew_count > 0)
{
coef = PI / a->hang_slew_count;
for (i = 0; i < a->hang_slew_count; i++)
a->hwave[i] = 0.5 * cos (i * coef);
}
flush_nob (a);
}
PORT
NOB create_nob (
int run,
int buffsize,
double* in,
double* out,
double samplerate,
int mode,
double advslewtime,
double advtime,
double hangslewtime,
double hangtime,
double max_imp_seq_time,
double backtau,
double threshold
)
{
NOB a = (NOB) malloc0 (sizeof (nob));
a->run = run;
a->buffsize = buffsize;
a->in = in;
a->out = out;
a->samplerate = samplerate;
a->mode = mode;
a->advslewtime = advslewtime;
a->advtime = advtime;
a->hangslewtime = hangslewtime;
a->hangtime = hangtime;
a->max_imp_seq_time = max_imp_seq_time;
a->backtau = backtau;
a->threshold = threshold;
a->dline_size = (int)(MAX_SAMPLERATE * (MAX_ADV_SLEW_TIME +
MAX_ADV_TIME +
MAX_HANG_SLEW_TIME +
MAX_HANG_TIME +
MAX_SEQ_TIME ) + 2);
a->dline = (double *)malloc0 (a->dline_size * sizeof (complex));
a->imp = (int *)malloc0 (a->dline_size * sizeof (int));
a->awave = (double *)malloc0 ((int)(MAX_ADV_SLEW_TIME * MAX_SAMPLERATE + 1) * sizeof (double));
a->hwave = (double *)malloc0 ((int)(MAX_HANG_SLEW_TIME * MAX_SAMPLERATE + 1) * sizeof (double));
a->filterlen = 10;
a->bfbuff = (double *)malloc0 (a->filterlen * sizeof (complex));
a->ffbuff = (double *)malloc0 (a->filterlen * sizeof (complex));
a->fcoefs = (double *)malloc0 (a->filterlen * sizeof (double));
a->fcoefs[0] = 0.308720593;
a->fcoefs[1] = 0.216104415;
a->fcoefs[2] = 0.151273090;
a->fcoefs[3] = 0.105891163;
a->fcoefs[4] = 0.074123814;
a->fcoefs[5] = 0.051886670;
a->fcoefs[6] = 0.036320669;
a->fcoefs[7] = 0.025424468;
a->fcoefs[8] = 0.017797128;
a->fcoefs[9] = 0.012457989;
InitializeCriticalSectionAndSpinCount (&a->cs_update, 2500);
init_nob (a);
a->legacy = (double *) malloc0 (2048 * sizeof (complex)); /////////////// legacy interface - remove
return a;
}
PORT
void destroy_nob (NOB a)
{
_aligned_free (a->legacy); /////////////// remove
_aligned_free (a->fcoefs);
_aligned_free (a->ffbuff);
_aligned_free (a->bfbuff);
_aligned_free (a->hwave);
_aligned_free (a->awave);
_aligned_free (a->imp);
_aligned_free (a->dline);
_aligned_free (a);
}
PORT
void flush_nob (NOB a)
{
a->out_idx = 0;
a->scan_idx = a->out_idx + a->adv_slew_count + a->adv_count + 1;
a->in_idx = a->scan_idx + a->max_imp_seq + a->hang_count + a->hang_slew_count + a->filterlen;
a->state = 0;
a->overflow = 0;
a->avg = 1.0;
a->bfb_in_idx = a->filterlen - 1;
a->ffb_in_idx = a->filterlen - 1;
memset (a->dline, 0, a->dline_size * sizeof (complex));
memset (a->imp, 0, a->dline_size * sizeof (int));
memset (a->bfbuff, 0, a->filterlen * sizeof (complex));
memset (a->ffbuff, 0, a->filterlen * sizeof (complex));
}
PORT
void xnob (NOB a)
{
double scale;
double mag;
int bf_idx;
int ff_idx;
int lidx, tidx;
int i, j, k;
int bfboutidx;
int ffboutidx;
int hcount;
int len;
int ffcount;
int staydown;
EnterCriticalSection (&a->cs_update);
if (a->run)
{
for (i = 0; i < a->buffsize; i++)
{
a->dline[2 * a->in_idx + 0] = a->in[2 * i + 0];
a->dline[2 * a->in_idx + 1] = a->in[2 * i + 1];
mag = sqrt(a->dline[2 * a->in_idx + 0] * a->dline[2 * a->in_idx + 0] + a->dline[2 * a->in_idx + 1] * a->dline[2 * a->in_idx + 1]);
a->avg = a->backmult * a->avg + a->ombackmult * mag;
if (mag > (a->avg * a->threshold))
a->imp[a->in_idx] = 1;
else
a->imp[a->in_idx] = 0;
if ((bf_idx = a->out_idx + a->adv_slew_count) >= a->dline_size) bf_idx -= a->dline_size;
if (a->imp[bf_idx] == 0)
{
if (++a->bfb_in_idx == a->filterlen) a->bfb_in_idx -= a->filterlen;
a->bfbuff[2 * a->bfb_in_idx + 0] = a->dline[2 * bf_idx + 0];
a->bfbuff[2 * a->bfb_in_idx + 1] = a->dline[2 * bf_idx + 1];
}
switch (a->state)
{
case 0: // normal output & impulse setup
{
a->out[2 * i + 0] = a->dline[2 * a->out_idx + 0];
a->out[2 * i + 1] = a->dline[2 * a->out_idx + 1];
a->Ilast = a->dline[2 * a->out_idx + 0];
a->Qlast = a->dline[2 * a->out_idx + 1];
if (a->imp[a->scan_idx] > 0)
{
a->time = 0;
if (a->adv_slew_count > 0)
a->state = 1;
else if (a->adv_count > 0)
a->state = 2;
else
a->state = 3;
tidx = a->scan_idx;
a->blank_count = 0;
do
{
hcount = 0;
while ((a->imp[tidx] > 0 || hcount > 0) && a->blank_count < a->max_imp_seq)
{
a->blank_count++;
if (hcount > 0) hcount--;
if (a->imp[tidx] > 0) hcount = a->hang_count + a->hang_slew_count;
if (++tidx >= a->dline_size) tidx -= a->dline_size;
}
j = 1;
len = 0;
lidx = tidx;
while (j <= a->adv_slew_count + a->adv_count && len == 0)
{
if (a->imp[lidx] == 1)
{
len = j;
tidx = lidx;
}
if (++lidx >= a->dline_size) lidx -= a->dline_size;
j++;
}
if((a->blank_count += len) > a->max_imp_seq)
{
a->blank_count = a->max_imp_seq;
a->overflow = 1;
break;
}
} while (len != 0);
if (a->overflow == 0)
{
a->blank_count -= a->hang_slew_count;
a->Inext = a->dline[2 * tidx + 0];
a->Qnext = a->dline[2 * tidx + 1];
if (a->mode == 1 || a->mode == 2 || a->mode == 4)
{
bfboutidx = a->bfb_in_idx;
a->I1 = 0.0;
a->Q1 = 0.0;
for (k = 0; k < a->filterlen; k++)
{
a->I1 += a->fcoefs[k] * a->bfbuff[2 * bfboutidx + 0];
a->Q1 += a->fcoefs[k] * a->bfbuff[2 * bfboutidx + 1];
if (--bfboutidx < 0) bfboutidx += a->filterlen;
}
}
if (a->mode == 2 || a->mode == 3 || a->mode == 4)
{
if ((ff_idx = a->scan_idx + a->blank_count) >= a->dline_size) ff_idx -= a->dline_size;
ffcount = 0;
while (ffcount < a->filterlen)
{
if (a->imp[ff_idx] == 0)
{
if (++a->ffb_in_idx == a->filterlen) a->ffb_in_idx -= a->filterlen;
a->ffbuff[2 * a->ffb_in_idx + 0] = a->dline[2 * ff_idx + 0];
a->ffbuff[2 * a->ffb_in_idx + 1] = a->dline[2 * ff_idx + 1];
++ffcount;
}
if (++ff_idx >= a->dline_size) ff_idx -= a->dline_size;
}
if ((ffboutidx = a->ffb_in_idx + 1) >= a->filterlen) ffboutidx -= a->filterlen;
a->I2 = 0.0;
a->Q2 = 0.0;
for (k = 0; k < a->filterlen; k++)
{
a->I2 += a->fcoefs[k] * a->ffbuff[2 * ffboutidx + 0];
a->Q2 += a->fcoefs[k] * a->ffbuff[2 * ffboutidx + 1];
if (++ffboutidx >= a->filterlen) ffboutidx -= a->filterlen;
}
}
switch (a->mode)
{
case 0: // zero
a->deltaI = 0.0;
a->deltaQ = 0.0;
a->I = 0.0;
a->Q = 0.0;
break;
case 1: // sample-hold
a->deltaI = 0.0;
a->deltaQ = 0.0;
a->I = a->I1;
a->Q = a->Q1;
break;
case 2: // mean-hold
a->deltaI = 0.0;
a->deltaQ = 0.0;
a->I = 0.5 * (a->I1 + a->I2);
a->Q = 0.5 * (a->Q1 + a->Q2);
break;
case 3: // hold-sample
a->deltaI = 0.0;
a->deltaQ = 0.0;
a->I = a->I2;
a->Q = a->Q2;
break;
case 4: // linear interpolation
a->deltaI = (a->I2 - a->I1) / (a->adv_count + a->blank_count);
a->deltaQ = (a->Q2 - a->Q1) / (a->adv_count + a->blank_count);
a->I = a->I1;
a->Q = a->Q1;
break;
}
}
else
{
if (a->adv_slew_count > 0)
a->state = 5;
else
{
a->state = 6;
a->time = 0;
a->blank_count += a->adv_count + a->filterlen;
}
}
}
break;
}
case 1: // slew output in advance of blanking period
{
scale = 0.5 + a->awave[a->time];
a->out[2 * i + 0] = a->Ilast * scale + (1.0 - scale) * a->I;
a->out[2 * i + 1] = a->Qlast * scale + (1.0 - scale) * a->Q;
if (++a->time == a->adv_slew_count)
{
a->time = 0;
if (a->adv_count > 0)
a->state = 2;
else
a->state = 3;
}
break;
}
case 2: // initial advance period
{
a->out[2 * i + 0] = a->I;
a->out[2 * i + 1] = a->Q;
a->I += a->deltaI;
a->Q += a->deltaQ;
if (++a->time == a->adv_count)
{
a->state = 3;
a->time = 0;
}
break;
}
case 3: // impulse & hang period
{
a->out[2 * i + 0] = a->I;
a->out[2 * i + 1] = a->Q;
a->I += a->deltaI;
a->Q += a->deltaQ;
if (++a->time == a->blank_count)
{
if (a->hang_slew_count > 0)
{
a->state = 4;
a->time = 0;
}
else
a->state = 0;
}
break;
}
case 4: // slew output after blanking period
{
scale = 0.5 - a->hwave[a->time];
a->out[2 * i + 0] = a->Inext * scale + (1.0 - scale) * a->I;
a->out[2 * i + 1] = a->Qnext * scale + (1.0 - scale) * a->Q;
if (++a->time == a->hang_slew_count)
a->state = 0;
break;
}
case 5:
{
scale = 0.5 + a->awave[a->time];
a->out[2 * i + 0] = a->Ilast * scale;
a->out[2 * i + 1] = a->Qlast * scale;
if (++a->time == a->adv_slew_count)
{
a->state = 6;
a->time = 0;
a->blank_count += a->adv_count + a->filterlen;
}
break;
}
case 6:
{
a->out[2 * i + 0] = 0.0;
a->out[2 * i + 1] = 0.0;
if (++a->time == a->blank_count)
a->state = 7;
break;
}
case 7:
{
a->out[2 * i + 0] = 0.0;
a->out[2 * i + 1] = 0.0;
staydown = 0;
a->time = 0;
if ((tidx = a->scan_idx + a->hang_slew_count + a->hang_count) >= a->dline_size) tidx -= a->dline_size;
while (a->time++ <= a->adv_count + a->adv_slew_count + a->hang_slew_count + a->hang_count) // CHECK EXACT COUNTS!!!!!!!!!!!!!!!!!!!!!!!
{
if (a->imp[tidx] == 1) staydown = 1;
if (--tidx < 0) tidx += a->dline_size;
}
if (staydown == 0)
{
if (a->hang_count > 0)
{
a->state = 8;
a->time = 0;
}
else if (a->hang_slew_count > 0)
{
a->state = 9;
a->time = 0;
if ((tidx = a->scan_idx + a->hang_slew_count + a->hang_count - a->adv_count - a->adv_slew_count) >= a->dline_size) tidx -= a->dline_size;
if (tidx < 0) tidx += a->dline_size;
a->Inext = a->dline[2 * tidx + 0];
a->Qnext = a->dline[2 * tidx + 1];
}
else
{
a->state = 0;
a->overflow = 0;
}
}
break;
}
case 8:
{
a->out[2 * i + 0] = 0.0;
a->out[2 * i + 1] = 0.0;
if (++a->time == a->hang_count)
{
if (a->hang_slew_count > 0)
{
a->state = 9;
a->time = 0;
if ((tidx = a->scan_idx + a->hang_slew_count - a->adv_count - a->adv_slew_count) >= a->dline_size) tidx -= a->dline_size;
if (tidx < 0) tidx += a->dline_size;
a->Inext = a->dline[2 * tidx + 0];
a->Qnext = a->dline[2 * tidx + 1];
}
else
{
a->state = 0;
a->overflow = 0;
}
}
break;
}
case 9:
{
scale = 0.5 - a->hwave[a->time];
a->out[2 * i + 0] = a->Inext * scale;
a->out[2 * i + 1] = a->Qnext * scale;
if (++a->time >= a->hang_slew_count)
{
a->state = 0;
a->overflow = 0;
}
break;
}
}
if (++a->in_idx == a->dline_size) a->in_idx = 0;
if (++a->scan_idx == a->dline_size) a->scan_idx = 0;
if (++a->out_idx == a->dline_size) a->out_idx = 0;
}
}
else if (a->in != a->out)
memcpy (a->out, a->in, a->buffsize * sizeof (complex));
LeaveCriticalSection (&a->cs_update);
}
void setBuffers_nob (NOB a, double* in, double* out)
{
a->in = in;
a->out = out;
}
void setSamplerate_nob (NOB a, int rate)
{
a->samplerate = rate;
init_nob (a);
}
void setSize_nob (NOB a, int size)
{
a->buffsize = size;
flush_nob (a);
}
/********************************************************************************************************
* *
* POINTER-BASED PROPERTIES *
* *
********************************************************************************************************/
PORT
void pSetRCVRNOBRun (NOB a, int run)
{
EnterCriticalSection (&a->cs_update);
a->run = run;
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBMode (NOB a, int mode)
{
EnterCriticalSection (&a->cs_update);
a->mode = mode;
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBBuffsize (NOB a, int size)
{
EnterCriticalSection (&a->cs_update);
a->buffsize = size;
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBSamplerate (NOB a, int rate)
{
EnterCriticalSection (&a->cs_update);
a->samplerate = (double) rate;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBTau (NOB a, double tau)
{
EnterCriticalSection (&a->cs_update);
a->advslewtime = tau;
a->hangslewtime = tau;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBHangtime (NOB a, double time)
{
EnterCriticalSection (&a->cs_update);
a->hangtime = time;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBAdvtime (NOB a, double time)
{
EnterCriticalSection (&a->cs_update);
a->advtime = time;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBBacktau (NOB a, double tau)
{
EnterCriticalSection (&a->cs_update);
a->backtau = tau;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void pSetRCVRNOBThreshold (NOB a, double thresh)
{
EnterCriticalSection (&a->cs_update);
a->threshold = thresh;
LeaveCriticalSection (&a->cs_update);
}
/********************************************************************************************************
* *
* CALLS FOR EXTERNAL USE *
* *
********************************************************************************************************/
#define MAX_EXT_NOBS (32) // maximum number of NOBs called from outside wdsp
__declspec (align (16)) NOB pnob[MAX_EXT_NOBS]; // array of pointers for NOBs used EXTERNAL to wdsp
PORT
void create_nobEXT (
int id,
int run,
int mode,
int buffsize,
double samplerate,
double slewtime,
double hangtime,
double advtime,
double backtau,
double threshold
)
{
double advslewtime = slewtime;
double hangslewtime = slewtime;
double max_imp_seq_time = 0.025;
pnob[id] = create_nob (run, buffsize, 0, 0, samplerate, mode, advslewtime, advtime, hangslewtime, hangtime, max_imp_seq_time, backtau, threshold);
}
PORT
void destroy_nobEXT (int id)
{
destroy_nob (pnob[id]);
}
PORT
void flush_nobEXT (int id)
{
flush_nob (pnob[id]);
}
PORT
void xnobEXT (int id, double* in, double* out)
{
NOB a = pnob[id];
a->in = in;
a->out = out;
xnob (a);
}
PORT
void SetEXTNOBRun (int id, int run)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->run = run;
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBMode (int id, int mode)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->mode = mode;
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBBuffsize (int id, int size)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->buffsize = size;
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBSamplerate (int id, int rate)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->samplerate = (double) rate;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBTau (int id, double tau)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->advslewtime = tau;
a->hangslewtime = tau;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBHangtime (int id, double time)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->hangtime = time;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBAdvtime (int id, double time)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->advtime = time;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBBacktau (int id, double tau)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->backtau = tau;
init_nob (a);
LeaveCriticalSection (&a->cs_update);
}
PORT
void SetEXTNOBThreshold (int id, double thresh)
{
NOB a = pnob[id];
EnterCriticalSection (&a->cs_update);
a->threshold = thresh;
LeaveCriticalSection (&a->cs_update);
}
/********************************************************************************************************
* *
* LEGACY INTERFACE *
* *
********************************************************************************************************/
PORT
void xnobEXTF (int id, float *I, float *Q)
{
int i;
NOB a = pnob[id];
a->in = a->legacy;
a->out = a->legacy;
for (i = 0; i < a->buffsize; i++)
{
a->legacy[2 * i + 0] = (double)I[i];
a->legacy[2 * i + 1] = (double)Q[i];
}
xnob (a);
for (i = 0; i < a->buffsize; i++)
{
I[i] = (float)a->legacy[2 * i + 0];
Q[i] = (float)a->legacy[2 * i + 1];
}
}