VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-04-06 11:39:02 -04:00
Code Issues Projects Releases Wiki Activity

WDSP: Sonar lint fixes (1)

Browse Source
This commit is contained in:
f4exb 2024-08-02 08:01:46 +02:00
parent 4ddb6dc9ff
commit 8941835466
20 changed files with 501 additions and 474 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
wdsp/RXA.cpp
View File

@ -354,7 +354,7 @@ RXA* RXA::create_rxa (
rxa->dsp_size, // buffer size
rxa->midbuff, // pointer to input buffer
rxa->midbuff, // pointer to output buffer
ANF_DLINE_SIZE, // dline_size
ANF::ANF_DLINE_SIZE, // dline_size
64, // taps
16, // delay
0.0001, // two_mu
@ -374,7 +374,7 @@ RXA* RXA::create_rxa (
rxa->dsp_size, // buffer size
rxa->midbuff, // pointer to input buffer
rxa->midbuff, // pointer to output buffer
ANR_DLINE_SIZE, // dline_size
ANR::ANR_DLINE_SIZE, // dline_size
64, // taps
16, // delay
0.0001, // two_mu

39
wdsp/amd.cpp
View File

@ -26,6 +26,7 @@ warren@wpratt.com
*/
#include <cmath>
#include <array>
#include "comm.hpp"
#include "amd.hpp"
@ -116,15 +117,19 @@ void AMD::flush()
void AMD::execute()
{
int i;
double audio;
double vco[2];
double corr[2];
std::array<double, 2> vco;
std::array<double, 2> corr;
double det;
double del_out;
double ai, bi, aq, bq;
double ai_ps, bi_ps, aq_ps, bq_ps;
int j, k;
double ai;
double bi;
double aq;
double bq;
double ai_ps;
double bi_ps;
double aq_ps;
double bq_ps;
if (run)
{
@ -133,7 +138,7 @@ void AMD::execute()
case 0: //AM Demodulator
{
for (i = 0; i < buff_size; i++)
for (int i = 0; i < buff_size; i++)
{
double xr = in_buff[2 * i + 0];
double xi = in_buff[2 * i + 1];
@ -146,8 +151,8 @@ void AMD::execute()
audio += dc_insert - dc;
}
out_buff[2 * i + 0] = audio;
out_buff[2 * i + 1] = audio;
out_buff[2 * i + 0] = (float) audio;
out_buff[2 * i + 1] = (float) audio;
}
break;
@ -155,7 +160,7 @@ void AMD::execute()
case 1: //Synchronous AM Demodulator with Sideband Separation
{
for (i = 0; i < buff_size; i++)
for (int i = 0; i < buff_size; i++)
{
vco[0] = cos(phs);
vco[1] = sin(phs);
@ -174,9 +179,9 @@ void AMD::execute()
dsI = ai;
dsQ = bq;
for (j = 0; j < STAGES; j++)
for (int j = 0; j < STAGES; j++)
{
k = 3 * j;
int k = 3 * j;
a[k + 3] = c0[j] * (a[k] - a[k + 5]) + a[k + 2];
b[k + 3] = c1[j] * (b[k] - b[k + 5]) + b[k + 2];
c[k + 3] = c0[j] * (c[k] - c[k + 5]) + c[k + 2];
@ -188,7 +193,7 @@ void AMD::execute()
bq_ps = c[OUT_IDX];
aq_ps = d[OUT_IDX];
for (j = OUT_IDX + 2; j > 0; j--)
for (int j = OUT_IDX + 2; j > 0; j--)
{
a[j] = a[j - 1];
b[j] = b[j - 1];
@ -217,6 +222,8 @@ void AMD::execute()
audio = (ai_ps + bi_ps) - (aq_ps - bq_ps);
break;
}
default:
break;
}
if (levelfade)
@ -226,8 +233,8 @@ void AMD::execute()
audio += dc_insert - dc;
}
out_buff[2 * i + 0] = audio;
out_buff[2 * i + 1] = audio;
out_buff[2 * i + 0] = (float) audio;
out_buff[2 * i + 1] = (float) audio;
if ((corr[0] == 0.0) && (corr[1] == 0.0))
corr[0] = 1.0;
@ -254,6 +261,8 @@ void AMD::execute()
break;
}
default:
break;
}
}
else if (in_buff != out_buff)

14
wdsp/amd.hpp
View File

@ -28,15 +28,6 @@ warren@wpratt.com
#ifndef wdsp_amd_hpp
#define wdsp_amd_hpp
// ff defines for sbdemod
#ifndef STAGES
#define STAGES 7
#endif
#ifndef OUT_IDX
#define OUT_IDX (3 * STAGES)
#endif
#include <array>
#include "export.h"
@ -61,13 +52,16 @@ public:
double phs; // pll - phase accumulator
double omega; // pll - locked pll frequency
double fil_out; // pll - filter output
double g1, g2; // pll - filter gain parameters
double g1; // pll - filter gain parameters
double g2; // pll - filter gain parameters
double tauR; // carrier removal time constant
double tauI; // carrier insertion time constant
double mtauR; // carrier removal multiplier
double onem_mtauR; // 1.0 - carrier_removal_multiplier
double mtauI; // carrier insertion multiplier
double onem_mtauI; // 1.0 - carrier_insertion_multiplier
static const int STAGES = 7;
static const int OUT_IDX = 3 * STAGES;
std::array<double, 3*STAGES + 3> a; // Filter a variables
std::array<double, 3*STAGES + 3> b; // Filter b variables
std::array<double, 3*STAGES + 3> c; // Filter c variables

65
wdsp/amsq.cpp
View File

@ -32,12 +32,12 @@ namespace WDSP {
void AMSQ::compute_slews()
{
int i;
double delta, theta;
double delta;
double theta;
delta = PI / (double)ntup;
theta = 0.0;
for (i = 0; i <= ntup; i++)
for (int i = 0; i <= ntup; i++)
{
cup[i] = muted_gain + (1.0 - muted_gain) * 0.5 * (1.0 - cos (theta));
theta += delta;
@ -46,7 +46,7 @@ void AMSQ::compute_slews()
delta = PI / (double)ntdown;
theta = 0.0;
for (i = 0; i <= ntdown; i++)
for (int i = 0; i <= ntdown; i++)
{
cdown[i] = muted_gain + (1.0 - muted_gain) * 0.5 * (1.0 + cos (theta));
theta += delta;
@ -63,11 +63,11 @@ void AMSQ::calc()
// level change
ntup = (int)(tup * rate);
ntdown = (int)(tdown * rate);
cup.resize((ntup + 1) * 2); // (float *)malloc0((ntup + 1) * sizeof(float));
cdown.resize((ntdown + 1) * 2); // (float *)malloc0((ntdown + 1) * sizeof(float));
cup.resize((ntup + 1) * 2);
cdown.resize((ntdown + 1) * 2);
compute_slews();
// control
state = 0;
state = AMSQState::MUTED;
}
AMSQ::AMSQ (
@ -108,26 +108,17 @@ void AMSQ::flush()
{
std::fill(trigsig.begin(), trigsig.end(), 0);
avsig = 0.0;
state = 0;
state = AMSQState::MUTED;
}
enum _amsqstate
{
MUTED,
INCREASE,
UNMUTED,
TAIL,
DECREASE
};
void AMSQ::execute()
{
if (run)
{
int i;
double sig, siglimit;
double sig;
double siglimit;
for (i = 0; i < size; i++)
for (int i = 0; i < size; i++)
{
double trigr = trigsig[2 * i + 0];
double trigi = trigsig[2 * i + 1];
@ -136,31 +127,31 @@ void AMSQ::execute()
switch (state)
{
case MUTED:
case AMSQState::MUTED:
if (avsig > unmute_thresh)
{
state = INCREASE;
state = AMSQState::INCREASE;
count = ntup;
}
out[2 * i + 0] = muted_gain * in[2 * i + 0];
out[2 * i + 1] = muted_gain * in[2 * i + 1];
out[2 * i + 0] = (float) (muted_gain * in[2 * i + 0]);
out[2 * i + 1] = (float) (muted_gain * in[2 * i + 1]);
break;
case INCREASE:
out[2 * i + 0] = in[2 * i + 0] * cup[ntup - count];
out[2 * i + 1] = in[2 * i + 1] * cup[ntup - count];
case AMSQState::INCREASE:
out[2 * i + 0] = (float) (in[2 * i + 0] * cup[ntup - count]);
out[2 * i + 1] = (float) (in[2 * i + 1] * cup[ntup - count]);
if (count-- == 0)
state = UNMUTED;
state = AMSQState::UNMUTED;
break;
case UNMUTED:
case AMSQState::UNMUTED:
if (avsig < tail_thresh)
{
state = TAIL;
state = AMSQState::TAIL;
if ((siglimit = avsig) > 1.0)
siglimit = 1.0;
@ -173,28 +164,28 @@ void AMSQ::execute()
break;
case TAIL:
case AMSQState::TAIL:
out[2 * i + 0] = in[2 * i + 0];
out[2 * i + 1] = in[2 * i + 1];
if (avsig > unmute_thresh)
{
state = UNMUTED;
state = AMSQState::UNMUTED;
}
else if (count-- == 0)
{
state = DECREASE;
state = AMSQState::TAIL;
count = ntdown;
}
break;
case DECREASE:
out[2 * i + 0] = in[2 * i + 0] * cdown[ntdown - count];
out[2 * i + 1] = in[2 * i + 1] * cdown[ntdown - count];
case AMSQState::DECREASE:
out[2 * i + 0] = (float) (in[2 * i + 0] * cdown[ntdown - count]);
out[2 * i + 1] = (float) (in[2 * i + 1] * cdown[ntdown - count]);
if (count-- == 0)
state = MUTED;
state = AMSQState::MUTED;
break;
}

11
wdsp/amsq.hpp
View File

@ -36,6 +36,15 @@ namespace WDSP {
class WDSP_API AMSQ
{
public:
enum class AMSQState
{
MUTED,
INCREASE,
UNMUTED,
TAIL,
DECREASE
};
int run; // 0 if squelch system is OFF; 1 if it's ON
int size; // size of input/output buffers
float* in; // squelch input signal buffer
@ -47,7 +56,7 @@ public:
double avm;
double onem_avm;
double avsig;
int state; // state machine control
AMSQState state; // state machine control
int count;
double tup;
double tdown;

55
wdsp/anb.cpp
View File

@ -36,7 +36,6 @@ namespace WDSP {
void ANB::initBlanker()
{
int i;
trans_count = (int)(tau * samplerate);
if (trans_count < 2)
@ -54,7 +53,7 @@ void ANB::initBlanker()
backmult = exp(-1.0 / (samplerate * backtau));
ombackmult = 1.0 - backmult;
for (i = 0; i <= trans_count; i++)
for (int i = 0; i <= trans_count; i++)
wave[i] = 0.5 * cos(i * coef);
std::fill(dline.begin(), dline.end(), 0);
@ -76,23 +75,44 @@ ANB::ANB (
buffsize(_buffsize),
in(_in),
out(_out),
dline_size((int)((MAX_TAU + MAX_ADVTIME) * MAX_SAMPLERATE) + 1),
samplerate(_samplerate),
tau(_tau),
hangtime(_hangtime),
advtime(_advtime),
backtau(_backtau),
threshold(_threshold),
dtime(0),
htime(0),
itime(0),
atime(0)
threshold(_threshold)
{
tau = tau < 0.0 ? 0.0 : (tau > MAX_TAU ? MAX_TAU : tau);
hangtime = hangtime < 0.0 ? 0.0 : (hangtime > MAX_ADVTIME ? MAX_ADVTIME : hangtime);
advtime = advtime < 0.0 ? 0.0 : (advtime > MAX_ADVTIME ? MAX_ADVTIME : advtime);
samplerate = samplerate < 0.0 ? 0.0 : (samplerate > MAX_SAMPLERATE ? MAX_SAMPLERATE : samplerate);
dtime = 0;
htime = 0;
itime = 0;
atime = 0;
if (tau < 0.0) {
tau = 0.0;
} else if (tau > MAX_TAU) {
tau = MAX_TAU;
}
if (hangtime < 0.0) {
hangtime = 0.0;
} else if (hangtime > MAX_ADVTIME) {
hangtime = MAX_ADVTIME;
}
if (advtime < 0.0) {
advtime = 0.0;
} else if (advtime > MAX_ADVTIME) {
advtime = MAX_ADVTIME;
}
if (samplerate < 0.0) {
samplerate = 0.0;
} else if (samplerate > MAX_SAMPLERATE) {
samplerate = MAX_SAMPLERATE;
}
wave.resize((int)(MAX_SAMPLERATE * MAX_TAU) + 1);
dline_size = (int)((MAX_TAU + MAX_ADVTIME) * MAX_SAMPLERATE) + 1;
dline.resize(dline_size * 2);
initBlanker();
}
@ -106,11 +126,10 @@ void ANB::execute()
{
double scale;
double mag;
int i;
if (run)
{
for (i = 0; i < buffsize; i++)
for (int i = 0; i < buffsize; i++)
{
double xr = in[2 * i + 0];
double xi = in[2 * i + 1];
@ -139,8 +158,8 @@ void ANB::execute()
case 1:
scale = power * (0.5 + wave[dtime]);
out[2 * i + 0] = dline[2 * out_idx + 0] * scale;
out[2 * i + 1] = dline[2 * out_idx + 1] * scale;
out[2 * i + 0] = (float) (dline[2 * out_idx + 0] * scale);
out[2 * i + 1] = (float) (dline[2 * out_idx + 1] * scale);
if (++dtime > trans_count)
{
@ -177,8 +196,8 @@ void ANB::execute()
case 4:
scale = 0.5 - wave[itime];
out[2 * i + 0] = dline[2 * out_idx + 0] * scale;
out[2 * i + 1] = dline[2 * out_idx + 1] * scale;
out[2 * i + 0] = (float) (dline[2 * out_idx + 0] * scale);
out[2 * i + 1] = (float) (dline[2 * out_idx + 1] * scale);
if (count > 0)
{

60
wdsp/anf.cpp
View File

@ -53,49 +53,53 @@ ANF::ANF(
double _den_mult,
double _lincr,
double _ldecr
)
) :
run(_run),
position(_position),
buff_size(_buff_size),
in_buff(_in_buff),
out_buff(_out_buff),
dline_size(_dline_size),
mask(_dline_size - 1),
n_taps(_n_taps),
delay(_delay),
two_mu(_two_mu),
gamma(_gamma),
lidx(_lidx),
lidx_min(_lidx_min),
lidx_max(_lidx_max),
ngamma(_ngamma),
den_mult(_den_mult),
lincr(_lincr),
ldecr(_ldecr)
{
run = _run;
position = _position;
buff_size = _buff_size;
in_buff = _in_buff;
out_buff = _out_buff;
dline_size = _dline_size;
mask = _dline_size - 1;
n_taps = _n_taps;
delay = _delay;
two_mu = _two_mu;
gamma = _gamma;
in_idx = 0;
lidx = _lidx;
lidx_min = _lidx_min;
lidx_max = _lidx_max;
ngamma = _ngamma;
den_mult = _den_mult;
lincr = _lincr;
ldecr = _ldecr;
std::fill(d.begin(), d.end(), 0);
std::fill(w.begin(), w.end(), 0);
}
void ANF::execute(int _position)
{
int i, j, idx;
double c0, c1;
double y, error, sigma, inv_sigp;
double nel, nev;
int idx;
double c0;
double c1;
double y;
double error;
double sigma;
double inv_sigp;
double nel;
double nev;
if (run && (position == _position))
{
for (i = 0; i < buff_size; i++)
for (int i = 0; i < buff_size; i++)
{
d[in_idx] = in_buff[2 * i + 0];
y = 0;
sigma = 0;
for (j = 0; j < n_taps; j++)
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
y += w[j] * d[idx];
@ -105,7 +109,7 @@ void ANF::execute(int _position)
inv_sigp = 1.0 / (sigma + 1e-10);
error = d[in_idx] - y;
out_buff[2 * i + 0] = error;
out_buff[2 * i + 0] = (float) error;
out_buff[2 * i + 1] = 0.0;
if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0)
@ -128,7 +132,7 @@ void ANF::execute(int _position)
c0 = 1.0 - two_mu * ngamma;
c1 = two_mu * error * inv_sigp;
for (j = 0; j < n_taps; j++)
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
w[j] = c0 * w[j] + c1 * d[idx];

3
wdsp/anf.hpp
View File

@ -32,8 +32,6 @@ warren@wpratt.com
#include "export.h"
#define ANF_DLINE_SIZE 2048
namespace WDSP {
class WDSP_API ANF
@ -50,6 +48,7 @@ public:
int delay;
double two_mu;
double gamma;
static const int ANF_DLINE_SIZE = 2048;
std::array<double, ANF_DLINE_SIZE> d;
std::array<double, ANF_DLINE_SIZE> w;
int in_idx;

21
wdsp/anr.cpp
View File

@ -80,21 +80,26 @@ ANR::ANR(
void ANR::execute(int _position)
{
int i, j, idx;
double c0, c1;
double y, error, sigma, inv_sigp;
double nel, nev;
int idx;
double c0;
double c1;
double y;
double error;
double sigma;
double inv_sigp;
double nel;
double nev;
if (run && (position == _position))
{
for (i = 0; i < buff_size; i++)
for (int i = 0; i < buff_size; i++)
{
d[in_idx] = in_buff[2 * i + 0];
y = 0;
sigma = 0;
for (j = 0; j < n_taps; j++)
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
y += w[j] * d[idx];
@ -104,7 +109,7 @@ void ANR::execute(int _position)
inv_sigp = 1.0 / (sigma + 1e-10);
error = d[in_idx] - y;
out_buff[2 * i + 0] = y;
out_buff[2 * i + 0] = (float) y;
out_buff[2 * i + 1] = 0.0;
if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0)
@ -127,7 +132,7 @@ void ANR::execute(int _position)
c0 = 1.0 - two_mu * ngamma;
c1 = two_mu * error * inv_sigp;
for (j = 0; j < n_taps; j++)
for (int j = 0; j < n_taps; j++)
{
idx = (in_idx + j + delay) & mask;
w[j] = c0 * w[j] + c1 * d[idx];

3
wdsp/anr.hpp
View File

@ -32,8 +32,6 @@ warren@wpratt.com
#include "export.h"
#define ANR_DLINE_SIZE 2048
namespace WDSP {
class WDSP_API ANR
@ -50,6 +48,7 @@ public:
int delay;
double two_mu;
double gamma;
static const int ANR_DLINE_SIZE = 2048;
std::array<double, ANR_DLINE_SIZE> d;
std::array<double, ANR_DLINE_SIZE> w;
int in_idx;

72
wdsp/bandpass.cpp
View File

@ -51,21 +51,21 @@ BANDPASS::BANDPASS(
int _samplerate,
int _wintype,
double _gain
)
{
) :
// NOTE: 'nc' must be >= 'size'
run = _run;
position = _position;
size = _size;
nc = _nc;
mp = _mp;
in = _in;
out = _out;
f_low = _f_low;
f_high = _f_high;
samplerate = _samplerate;
wintype = _wintype;
gain = _gain;
run(_run),
position(_position),
size(_size),
nc(_nc),
mp(_mp),
in(_in),
out(_out),
f_low(_f_low),
f_high(_f_high),
samplerate(_samplerate),
wintype(_wintype),
gain(_gain)
{
float* impulse = FIR::fir_bandpass (
nc,
f_low,
@ -136,7 +136,7 @@ void BANDPASS::setSize(int _size)
gain / (double) (2 * size)
);
FIRCORE::setImpulse_fircore (fircore, impulse, 1);
delete[] (impulse);
delete[] impulse;
}
void BANDPASS::setGain(double _gain, int _update)
@ -152,7 +152,7 @@ void BANDPASS::setGain(double _gain, int _update)
gain / (double) (2 * size)
);
FIRCORE::setImpulse_fircore (fircore, impulse, _update);
delete[] (impulse);
delete[] impulse;
}
void BANDPASS::calcBandpassFilter(double _f_low, double _f_high, double _gain)
@ -172,7 +172,7 @@ void BANDPASS::calcBandpassFilter(double _f_low, double _f_high, double _gain)
gain / (double)(2 * size)
);
FIRCORE::setImpulse_fircore (fircore, impulse, 1);
delete[] (impulse);
delete[] impulse;
}
}
@ -197,7 +197,7 @@ void BANDPASS::setBandpassFreqs(double _f_low, double _f_high)
);
FIRCORE::setImpulse_fircore (fircore, impulse, 0);
delete[] (impulse);
delete[] impulse;
f_low = _f_low;
f_high = _f_high;
FIRCORE::setUpdate_fircore (fircore);
@ -220,7 +220,7 @@ void BANDPASS::SetBandpassNC(int _nc)
gain / (double)( 2 * size)
);
FIRCORE::setNc_fircore (fircore, nc, impulse);
delete[] (impulse);
delete[] impulse;
}
}
@ -239,38 +239,4 @@ void BANDPASS::SetBandpassMP(int _mp)
* *
********************************************************************************************************/
//PORT
//void SetTXABandpassFreqs (int channel, float f_low, float f_high)
//{
// float* impulse;
// BANDPASS a;
// a = txa.bp0;
// if ((f_low != a->f_low) || (f_high != a->f_high))
// {
// a->f_low = f_low;
// a->f_high = f_high;
// impulse = fir_bandpass (a->nc, a->f_low, a->f_high, a->samplerate, a->wintype, 1, a->gain / (float)(2 * a->size));
// setImpulse_fircore (a->fircore, impulse, 1);
// delete[] (impulse);
// }
// a = txa.bp1;
// if ((f_low != a->f_low) || (f_high != a->f_high))
// {
// a->f_low = f_low;
// a->f_high = f_high;
// impulse = fir_bandpass (a->nc, a->f_low, a->f_high, a->samplerate, a->wintype, 1, a->gain / (float)(2 * a->size));
// setImpulse_fircore (a->fircore, impulse, 1);
// delete[] (impulse);
// }
// a = txa.bp2;
// if ((f_low != a->f_low) || (f_high != a->f_high))
// {
// a->f_low = f_low;
// a->f_high = f_high;
// impulse = fir_bandpass (a->nc, a->f_low, a->f_high, a->samplerate, a->wintype, 1, a->gain / (float)(2 * a->size));
// setImpulse_fircore (a->fircore, impulse, 1);
// delete[] (impulse);
// }
//}
} // namespace WDSP

11
wdsp/bpsnba.cpp
View File

@ -52,7 +52,7 @@ namespace WDSP {
void BPSNBA::calc()
{
buff = new float[size * 2]; // (double *) malloc0 (size * sizeof (complex));
buff.resize(size * 2);
bpsnba = new NBP (
1, // run, always runs (use bpsnba 'run')
run_notches, // run the notches
@ -60,7 +60,7 @@ void BPSNBA::calc()
size, // buffer size
nc, // number of filter coefficients
mp, // minimum phase flag
buff, // pointer to input buffer
buff.data(), // pointer to input buffer
out, // pointer to output buffer
f_low, // lower filter frequency
f_high, // upper filter frequency
@ -116,8 +116,7 @@ BPSNBA::BPSNBA(
void BPSNBA::decalc()
{
delete (bpsnba);
delete[] (buff);
delete bpsnba;
}
BPSNBA::~BPSNBA()
@ -127,7 +126,7 @@ BPSNBA::~BPSNBA()
void BPSNBA::flush()
{
std::fill(buff, buff + size * 2, 0);
std::fill(buff.begin(), buff.end(), 0);
bpsnba->flush();
}
@ -156,7 +155,7 @@ void BPSNBA::setSize(int _size)
void BPSNBA::exec_in(int _position)
{
if (run && position == _position)
std::copy(in, in + size * 2, buff);
std::copy(in, in + size * 2, buff.begin());
}
void BPSNBA::exec_out(int _position)

7
wdsp/bpsnba.hpp
View File

@ -28,12 +28,15 @@ warren@wpratt.com
#ifndef wdsp_bpsnba_h
#define wdsp_bpsnba_h
#include <vector>
#include "export.h"
namespace WDSP{
class NOTCHDB;
class NBP;
class BPSNBA
class WDSP_API BPSNBA
{
public:
int run; // run the filter
@ -49,7 +52,7 @@ public:
double abs_high_freq; // highest positive freq supported by SNG
double f_low; // low cutoff frequency
double f_high; // high cutoff frequency
float* buff; // internal buffer
std::vector<float> buff; // internal buffer
int wintype; // filter window type
double gain; // filter gain
int autoincr; // use auto increment for notch width

16
wdsp/cblock.cpp
View File

@ -71,22 +71,24 @@ void CBL::execute()
{
if (run)
{
int i;
double tempI, tempQ;
double tempI;
double tempQ;
for (i = 0; i < buff_size; i++)
for (int i = 0; i < buff_size; i++)
{
tempI = in_buff[2 * i + 0];
tempQ = in_buff[2 * i + 1];
out_buff[2 * i + 0] = in_buff[2 * i + 0] - prevIin + mtau * prevIout;
out_buff[2 * i + 1] = in_buff[2 * i + 1] - prevQin + mtau * prevQout;
out_buff[2 * i + 0] = (float) (in_buff[2 * i + 0] - prevIin + mtau * prevIout);
out_buff[2 * i + 1] = (float) (in_buff[2 * i + 1] - prevQin + mtau * prevQout);
prevIin = tempI;
prevQin = tempQ;
prevIout = out_buff[2 * i + 0];
prevQout = out_buff[2 * i + 1];
if (fabs(prevIout = out_buff[2 * i + 0]) < 1.0e-20)
if (fabs(prevIout) < 1.0e-20)
prevIout = 0.0;
if (fabs(prevQout = out_buff[2 * i + 1]) < 1.0e-20)
if (fabs(prevQout) < 1.0e-20)
prevQout = 0.0;
}
}

2
wdsp/dsphp.cpp
View File

@ -97,7 +97,7 @@ void DSPHP::execute()
x1[n] = x0[n];
}
out[i] = y0[nstages - 1];
out[i] = (float) y0[nstages - 1];
}
}
else if (out != in)

9
wdsp/dsphp.hpp
View File

@ -50,8 +50,13 @@ public:
double rate;
double fc;
int nstages;
double a1, b0, b1;
std::vector<double> x0, x1, y0, y1;
double a1;
double b0;
double b1;
std::vector<double> x0;
std::vector<double> x1;
std::vector<double> y0;
std::vector<double> y1;
DSPHP(
int run,

316
wdsp/emnr.cpp
View File

@ -72,10 +72,10 @@ EMNR::NPS::NPS(
epsH1(_epsH1)
{
epsH1r = epsH1 / (1.0 + epsH1);
sigma2N.resize(msize); // (float *)malloc0(nps.msize * sizeof(float));
PH1y.resize(msize); // (float *)malloc0(nps.msize * sizeof(float));
Pbar.resize(msize); // (float *)malloc0(nps.msize * sizeof(float));
EN2y.resize(msize); // (float *)malloc0(nps.msize * sizeof(float));
sigma2N.resize(msize);
PH1y.resize(msize);
Pbar.resize(msize);
EN2y.resize(msize);
for (int i = 0; i < msize; i++)
{
@ -86,9 +86,8 @@ EMNR::NPS::NPS(
void EMNR::NPS::LambdaDs()
{
int k;
for (k = 0; k < msize; k++)
for (int k = 0; k < msize; k++)
{
PH1y[k] = 1.0 / (1.0 + (1.0 + epsH1) * exp (- epsH1r * lambda_y[k] / sigma2N[k]));
Pbar[k] = alpha_Pbar * Pbar[k] + (1.0 - alpha_Pbar) * PH1y[k];
@ -122,31 +121,17 @@ EMNR::NP::NP(
lambda_d(_lambda_d)
{
{
double tau = -128.0 / 8000.0 / log(0.7);
alphaCsmooth = exp(-incr / rate / tau);
}
{
double tau = -128.0 / 8000.0 / log(0.96);
alphaMax = exp(-incr / rate / tau);
}
{
double tau = -128.0 / 8000.0 / log(0.7);
alphaCmin = exp(-incr / rate / tau);
}
{
double tau = -128.0 / 8000.0 / log(0.3);
alphaMin_max_value = exp(-incr / rate / tau);
}
double tau0 = -128.0 / 8000.0 / log(0.7);
alphaCsmooth = exp(-incr / rate / tau0);
double tau1 = -128.0 / 8000.0 / log(0.96);
alphaMax = exp(-incr / rate / tau1);
double tau2 = -128.0 / 8000.0 / log(0.7);
alphaCmin = exp(-incr / rate / tau2);
double tau3 = -128.0 / 8000.0 / log(0.3);
alphaMin_max_value = exp(-incr / rate / tau3);
snrq = -incr / (0.064 * rate);
double tau4 = -128.0 / 8000.0 / log(0.8);
betamax = exp(-incr / rate / tau4);
invQeqMax = 0.5;
av = 2.12;
Dtime = 8.0 * 12.0 * 128.0 / 8000.0;
@ -166,68 +151,63 @@ EMNR::NP::NP(
invQbar_points[0] = 0.03;
invQbar_points[1] = 0.05;
invQbar_points[2] = 0.06;
invQbar_points[3] = std::numeric_limits<double>::max();;
invQbar_points[3] = std::numeric_limits<double>::max();
{
double db;
db = 10.0 * log10(8.0) / (12.0 * 128 / 8000);
nsmax[0] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(4.0) / (12.0 * 128 / 8000);
nsmax[1] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(2.0) / (12.0 * 128 / 8000);
nsmax[2] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(1.2) / (12.0 * 128 / 8000);
nsmax[3] = pow(10.0, db / 10.0 * V * incr / rate);
}
double db;
db = 10.0 * log10(8.0) / (12.0 * 128 / 8000);
nsmax[0] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(4.0) / (12.0 * 128 / 8000);
nsmax[1] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(2.0) / (12.0 * 128 / 8000);
nsmax[2] = pow(10.0, db / 10.0 * V * incr / rate);
db = 10.0 * log10(1.2) / (12.0 * 128 / 8000);
nsmax[3] = pow(10.0, db / 10.0 * V * incr / rate);
p.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
alphaOptHat.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
alphaHat.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
sigma2N.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
pbar.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
p2bar.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
Qeq.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
bmin.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
bmin_sub.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
k_mod.resize(msize); // (int *)malloc0(np.msize * sizeof(int));
actmin.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
actmin_sub.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
lmin_flag.resize(msize); // (int *)malloc0(np.msize * sizeof(int));
pmin_u.resize(msize); // (float *)malloc0(np.msize * sizeof(float));
actminbuff.resize(U); // (float**)malloc0(np.U * sizeof(float*));
p.resize(msize);
alphaOptHat.resize(msize);
alphaHat.resize(msize);
sigma2N.resize(msize);
pbar.resize(msize);
p2bar.resize(msize);
Qeq.resize(msize);
bmin.resize(msize);
bmin_sub.resize(msize);
k_mod.resize(msize);
actmin.resize(msize);
actmin_sub.resize(msize);
lmin_flag.resize(msize);
pmin_u.resize(msize);
actminbuff.resize(U);
for (int i = 0; i < U; i++) {
actminbuff[i].resize(msize); // (float *)malloc0(np.msize * sizeof(float));
actminbuff[i].resize(msize);
}
alphaC = 1.0;
subwc = V;
amb_idx = 0;
for (int k = 0; k < msize; k++) {
lambda_y[k] = 0.5;
}
std::copy(lambda_y.begin(), lambda_y.end(), p.begin());
std::copy(lambda_y.begin(), lambda_y.end(), sigma2N.begin());
std::copy(lambda_y.begin(), lambda_y.end(), pbar.begin());
std::copy(lambda_y.begin(), lambda_y.end(), pmin_u.begin());
for (int k = 0; k < msize; k++)
{
int k, ku;
alphaC = 1.0;
subwc = V;
amb_idx = 0;
p2bar[k] = lambda_y[k] * lambda_y[k];
actmin[k] = std::numeric_limits<double>::max();
actmin_sub[k] = std::numeric_limits<double>::max();
for (k = 0; k < msize; k++) {
lambda_y[k] = 0.5;
for (int ku = 0; ku < U; ku++) {
actminbuff[ku][k] = std::numeric_limits<double>::max();
}
std::copy(lambda_y.begin(), lambda_y.end(), p.begin());
std::copy(lambda_y.begin(), lambda_y.end(), sigma2N.begin());
std::copy(lambda_y.begin(), lambda_y.end(), pbar.begin());
std::copy(lambda_y.begin(), lambda_y.end(), pmin_u.begin());
for (k = 0; k < msize; k++)
{
p2bar[k] = lambda_y[k] * lambda_y[k];
actmin[k] = std::numeric_limits<double>::max();
actmin_sub[k] = std::numeric_limits<double>::max();;
for (ku = 0; ku < U; ku++) {
actminbuff[ku][k] = std::numeric_limits<double>::max();;
}
}
std::fill(lmin_flag.begin(), lmin_flag.end(), 0);
}
std::fill(lmin_flag.begin(), lmin_flag.end(), 0);
}
void EMNR::NP::interpM (
@ -249,7 +229,9 @@ void EMNR::NP::interpM (
else
{
int idx = 1;
double xllow, xlhigh, frac;
double xllow;
double xlhigh;
double frac;
while ((x >= xvals[idx]) && (idx < nvals - 1))
idx++;
@ -264,16 +246,23 @@ void EMNR::NP::interpM (
void EMNR::NP::LambdaD()
{
int k;
double f0, f1, f2, f3;
double f0;
double f1;
double f2;
double f3;
double sum_prev_p;
double sum_lambda_y;
double alphaCtilda;
double sum_prev_sigma2N;
double alphaMin, SNR;
double beta, varHat, invQeq;
double alphaMin;
double SNR;
double beta;
double varHat;
double invQeq;
double invQbar;
double bc;
double QeqTilda, QeqTildaSub;
double QeqTilda;
double QeqTildaSub;
double noise_slope_max;
sum_prev_p = 0.0;
@ -369,7 +358,7 @@ void EMNR::NP::LambdaD()
lmin_flag[k] = 0;
actminbuff[amb_idx][k] = actmin[k];
min = std::numeric_limits<double>::max();;
min = std::numeric_limits<double>::max();
for (ku = 0; ku < U; ku++)
{
@ -389,8 +378,8 @@ void EMNR::NP::LambdaD()
}
lmin_flag[k] = 0;
actmin[k] = std::numeric_limits<double>::max();;
actmin_sub[k] = std::numeric_limits<double>::max();;
actmin[k] = std::numeric_limits<double>::max();
actmin_sub[k] = std::numeric_limits<double>::max();
}
if (++amb_idx == U)
@ -433,17 +422,15 @@ EMNR::G::G(
y(_y)
{
lambda_y.resize(msize); // (float *)malloc0(msize * sizeof(float));
lambda_d.resize(msize); // (float *)malloc0(msize * sizeof(float));
prev_gamma.resize(msize); // (float *)malloc0(msize * sizeof(float));
prev_mask.resize(msize); // (float *)malloc0(msize * sizeof(float));
lambda_y.resize(msize);
lambda_d.resize(msize);
prev_gamma.resize(msize);
prev_mask.resize(msize);
gf1p5 = sqrt(PI) / 2.0;
{
double tau = -128.0 / 8000.0 / log(0.98);
alpha = exp(-incr / rate / tau);
}
double tau = -128.0 / 8000.0 / log(0.98);
alpha = exp(-incr / rate / tau);
eps_floor = std::numeric_limits<double>::min();
gamma_max = 1000.0;
@ -480,7 +467,9 @@ EMNR::G::G(
void EMNR::G::calc_gamma0()
{
double gamma, eps_hat, v;
double gamma;
double eps_hat;
double v;
for (int k = 0; k < msize; k++)
{
@ -490,20 +479,15 @@ void EMNR::G::calc_gamma0()
v = (eps_hat / (1.0 + eps_hat)) * gamma;
mask[k] = gf1p5 * sqrt (v) / gamma * exp (- 0.5 * v)
* ((1.0 + v) * bessI0 (0.5 * v) + v * bessI1 (0.5 * v));
{
double v2 = std::min (v, 700.0);
double eta = mask[k] * mask[k] * lambda_y[k] / lambda_d[k];
double eps = eta / (1.0 - q);
double witchHat = (1.0 - q) / q * exp (v2) / (1.0 + eps);
mask[k] *= witchHat / (1.0 + witchHat);
}
double v2 = std::min (v, 700.0);
double eta = mask[k] * mask[k] * lambda_y[k] / lambda_d[k];
double eps = eta / (1.0 - q);
double witchHat = (1.0 - q) / q * exp (v2) / (1.0 + eps);
mask[k] *= witchHat / (1.0 + witchHat);
if (mask[k] > gmax)
mask[k] = gmax;
if (mask[k] != mask[k])
mask[k] = 0.01;
prev_gamma[k] = gamma;
prev_mask[k] = mask[k];
}
@ -511,7 +495,10 @@ void EMNR::G::calc_gamma0()
void EMNR::G::calc_gamma1()
{
double gamma, eps_hat, v, ehr;
double gamma;
double eps_hat;
double v;
double ehr;
for (int k = 0; k < msize; k++)
{
@ -524,9 +511,6 @@ void EMNR::G::calc_gamma1()
if ((mask[k] = ehr * exp (std::min (700.0, 0.5 * e1xb(v)))) > gmax)
mask[k] = gmax;
if (mask[k] != mask[k])
mask[k] = 0.01;
prev_gamma[k] = gamma;
prev_mask[k] = mask[k];
}
@ -534,7 +518,9 @@ void EMNR::G::calc_gamma1()
void EMNR::G::calc_gamma2()
{
double gamma, eps_hat, eps_p;
double gamma;
double eps_hat;
double eps_p;
for (int k = 0; k < msize; k++)
{
@ -572,7 +558,8 @@ void EMNR::G::calc_lambda_y()
double EMNR::G::bessI0 (double x)
{
double res, p;
double res;
double p;
if (x == 0.0)
{
@ -616,7 +603,8 @@ double EMNR::G::bessI0 (double x)
double EMNR::G::bessI1 (double x)
{
double res, p;
double res;
double p;
if (x == 0.0)
{
@ -667,12 +655,17 @@ double EMNR::G::bessI1 (double x)
double EMNR::G::e1xb (double x)
{
double e1, ga, r, t, t0;
int k, m;
double e1;
double ga;
double r;
double t;
double t0;
int k;
int m;
if (x == 0.0)
{
e1 = std::numeric_limits<double>::max();;
e1 = std::numeric_limits<double>::max();
}
else if (x <= 1.0)
{
@ -715,26 +708,25 @@ double EMNR::G::e1xb (double x)
void EMNR::calc_window()
{
int i;
double arg, sum, inv_coherent_gain;
double arg;
double sum;
double inv_coherent_gain;
switch (wintype)
if (wintype == 0)
{
case 0:
arg = 2.0 * PI / (double) fsize;
sum = 0.0;
for (i = 0; i < fsize; i++)
{
window[i] = sqrt (0.54 - 0.46 * cos((float)i * arg));
window[i] = (float) (sqrt (0.54 - 0.46 * cos((float)i * arg)));
sum += window[i];
}
inv_coherent_gain = (double) fsize / sum;
for (i = 0; i < fsize; i++)
window[i] *= inv_coherent_gain;
break;
window[i] *= (float) inv_coherent_gain;
}
}
@ -771,20 +763,20 @@ void EMNR::calc()
init_oainidx = oainidx;
oaoutidx = 0;
msize = fsize / 2 + 1;
window.resize(fsize); // (float *)malloc0(fsize * sizeof(float));
inaccum.resize(iasize); // (float *)malloc0(iasize * sizeof(float));
forfftin.resize(fsize); // (float *)malloc0(fsize * sizeof(float));
forfftout.resize(msize * 2); // (float *)malloc0(msize * sizeof(complex));
mask.resize(msize); // (float *)malloc0(msize * sizeof(float));
window.resize(fsize);
inaccum.resize(iasize);
forfftin.resize(fsize);
forfftout.resize(msize * 2);
mask.resize(msize);
std::fill(mask.begin(), mask.end(), 1.0);
revfftin.resize(msize * 2); // (float *)malloc0(msize * sizeof(complex));
revfftout.resize(fsize); // (float *)malloc0(fsize * sizeof(float));
save.resize(ovrlp); // (float **)malloc0(ovrlp * sizeof(float *));
revfftin.resize(msize * 2);
revfftout.resize(fsize);
save.resize(ovrlp);
for (int i = 0; i < ovrlp; i++)
save[i].resize(fsize); // (float *)malloc0(fsize * sizeof(float));
save[i].resize(fsize);
outaccum.resize(oasize); // (float *)malloc0(oasize * sizeof(float));
outaccum.resize(oasize);
nsamps = 0;
saveidx = 0;
Rfor = fftwf_plan_dft_r2c_1d(
@ -853,10 +845,10 @@ void EMNR::calc()
void EMNR::decalc()
{
delete (ae);
delete (nps);
delete (np);
delete (g);
delete ae;
delete nps;
delete np;
delete g;
fftwf_destroy_plan(Rrev);
fftwf_destroy_plan(Rfor);
@ -896,10 +888,9 @@ EMNR::EMNR(
void EMNR::flush()
{
int i;
std::fill(inaccum.begin(), inaccum.end(), 0);
for (i = 0; i < ovrlp; i++)
for (int i = 0; i < ovrlp; i++)
std::fill(save[i].begin(), save[i].end(), 0);
std::fill(outaccum.begin(), outaccum.end(), 0);
@ -918,9 +909,13 @@ EMNR::~EMNR()
void EMNR::aepf()
{
int k, m;
int N, n;
double sumPre, sumPost, zeta, zetaT;
int k;
int N;
int n;
double sumPre;
double sumPost;
double zeta;
double zetaT;
sumPre = 0.0;
sumPost = 0.0;
@ -947,7 +942,7 @@ void EMNR::aepf()
for (k = n; k < (ae->msize - n); k++)
{
ae->nmask[k] = 0.0;
for (m = k - n; m <= (k + n); m++)
for (int m = k - n; m <= (k + n); m++)
ae->nmask[k] += mask[m];
ae->nmask[k] /= (float)N;
}
@ -957,8 +952,14 @@ void EMNR::aepf()
double EMNR::G::getKey(const std::array<double, 241*241>& type, double gamma, double xi)
{
int ngamma1, ngamma2, nxi1 = 0, nxi2 = 0;
double tg, tx, dg, dx;
int ngamma1;
int ngamma2;
int nxi1 = 0;
int nxi2 = 0;
double tg;
double tx;
double dg;
double dx;
const double dmin = 0.001;
const double dmax = 1000.0;
@ -1005,8 +1006,13 @@ double EMNR::G::getKey(const std::array<double, 241*241>& type, double gamma, do
ix[2] = 241 * nxi1 + ngamma2;
ix[3] = 241 * nxi2 + ngamma2;
for (auto& ixi : ix) {
ixi = ixi < 0 ? 0 : (ixi >= 241*241 ? 241*241 - 1 : ixi);
for (auto& ixi : ix)
{
if (ixi < 0) {
ixi = 0;
} else if (ixi >= 241*241) {
ixi = 241*241 - 1;
}
}
return (1.0 - dg) * (1.0 - dx) * type[ix[0]]
@ -1027,6 +1033,8 @@ void EMNR::calc_gain()
case 1:
nps->LambdaDs();
break;
default:
break;
}
switch (g->gain_method)
@ -1040,6 +1048,8 @@ void EMNR::calc_gain()
case 2:
g->calc_gamma2();
break;
default:
break;
}
if (g->ae_run)
@ -1050,7 +1060,11 @@ void EMNR::execute(int _pos)
{
if (run && _pos == position)
{
int i, j, k, sbuff, sbegin;
int i;
int j;
int k;
int sbuff;
int sbegin;
double g1;
for (i = 0; i < 2 * bsize; i += 2)
@ -1074,8 +1088,8 @@ void EMNR::execute(int _pos)
for (i = 0; i < msize; i++)
{
g1 = gain * mask[i];
revfftin[2 * i + 0] = g1 * forfftout[2 * i + 0];
revfftin[2 * i + 1] = g1 * forfftout[2 * i + 1];
revfftin[2 * i + 0] = (float) (g1 * forfftout[2 * i + 0]);
revfftin[2 * i + 1] = (float) (g1 * forfftout[2 * i + 1]);
}
fftwf_execute (Rrev);

13
wdsp/emnr.hpp
View File

@ -119,7 +119,8 @@ public:
static double e1xb (double x);
static double bessI0 (double x);
static double bessI1 (double x);
} *g;
};
G *g;
struct NP
{
@ -186,7 +187,8 @@ public:
const std::array<double, 18>& xvals,
const std::array<double, 18>& yvals
);
} *np;
};
NP *np;
struct NPS
{
@ -221,8 +223,8 @@ public:
~NPS() = default;
void LambdaDs();
} *nps;
};
NPS *nps;
struct AE
{
int msize;
@ -240,7 +242,8 @@ public:
AE(const AE&) = delete;
AE& operator=(const AE& other) = delete;
~AE() = default;
} *ae;
};
AE *ae;
EMNR(
int run,

250
wdsp/fir.cpp
View File

@ -27,6 +27,7 @@ warren@pratt.one
#define _CRT_SECURE_NO_WARNINGS
#include <limits>
#include <vector>
#include "fftw3.h"
#include "comm.hpp"
@ -36,132 +37,131 @@ namespace WDSP {
float* FIR::fftcv_mults (int NM, float* c_impulse)
{
float* mults = new float[NM * 2];
float* cfft_impulse = new float[NM * 2];
auto mults = new float[NM * 2];
std::vector<float> cfft_impulse(NM * 2);
fftwf_plan ptmp = fftwf_plan_dft_1d(
NM,
(fftwf_complex *) cfft_impulse,
(fftwf_complex *) cfft_impulse.data(),
(fftwf_complex *) mults,
FFTW_FORWARD,
FFTW_PATIENT
);
std::fill(cfft_impulse, cfft_impulse + NM * 2, 0);
std::fill(cfft_impulse.begin(), cfft_impulse.end(), 0);
// store complex coefs right-justified in the buffer
std::copy(c_impulse, c_impulse + (NM / 2 + 1) * 2, &(cfft_impulse[NM - 2]));
fftwf_execute (ptmp);
fftwf_destroy_plan (ptmp);
delete[] cfft_impulse;
return mults;
}
float* FIR::get_fsamp_window(int N, int wintype)
{
int i;
double arg0, arg1;
float* window = new float[N]; // (float *) malloc0 (N * sizeof(float));
double arg0;
double arg1;
auto window = new float[N];
switch (wintype)
{
case 0:
arg0 = 2.0 * PI / ((double)N - 1.0);
for (i = 0; i < N; i++)
for (int i = 0; i < N; i++)
{
arg1 = cos(arg0 * (double)i);
window[i] = +0.21747
double val = +0.21747
+ arg1 * (-0.45325
+ arg1 * (+0.28256
+ arg1 * (-0.04672)));
window[i] = (float) val;
}
break;
case 1:
arg0 = 2.0 * PI / ((double)N - 1.0);
for (i = 0; i < N; ++i)
for (int i = 0; i < N; ++i)
{
arg1 = cos(arg0 * (double)i);
window[i] = +6.3964424114390378e-02
double val = +6.3964424114390378e-02
+ arg1 * (-2.3993864599352804e-01
+ arg1 * (+3.5015956323820469e-01
+ arg1 * (-2.4774111897080783e-01
+ arg1 * (+8.5438256055858031e-02
+ arg1 * (-1.2320203369293225e-02
+ arg1 * (+4.3778825791773474e-04))))));
window[i] = (float) val;
}
break;
default:
for (i = 0; i < N; i++)
for (int i = 0; i < N; i++)
window[i] = 1.0;
}
return window;
}
float* FIR::fir_fsamp_odd (int N, float* A, int rtype, double scale, int wintype)
float* FIR::fir_fsamp_odd (int N, const float* A, int rtype, double scale, int wintype)
{
int i, j;
int mid = (N - 1) / 2;
double mag, phs;
float* window;
float *fcoef = new float[N * 2];
float *c_impulse = new float[N * 2];
double mag;
double phs;
std::vector<float> fcoef(N * 2);
auto *c_impulse = new float[N * 2];
fftwf_plan ptmp = fftwf_plan_dft_1d(
N,
(fftwf_complex *)fcoef,
(fftwf_complex *)fcoef.data(),
(fftwf_complex *)c_impulse,
FFTW_BACKWARD,
FFTW_PATIENT
);
double local_scale = 1.0 / (double) N;
for (i = 0; i <= mid; i++)
for (int i = 0; i <= mid; i++)
{
mag = A[i] * local_scale;
phs = - (double)mid * TWOPI * (double)i / (double)N;
fcoef[2 * i + 0] = mag * cos (phs);
fcoef[2 * i + 1] = mag * sin (phs);
fcoef[2 * i + 0] = (float) (mag * cos (phs));
fcoef[2 * i + 1] = (float) (mag * sin (phs));
}
for (i = mid + 1, j = 0; i < N; i++, j++)
for (int i = mid + 1, j = 0; i < N; i++, j++)
{
fcoef[2 * i + 0] = + fcoef[2 * (mid - j) + 0];
fcoef[2 * i + 1] = - fcoef[2 * (mid - j) + 1];
}
fftwf_execute (ptmp);
fftwf_destroy_plan (ptmp);
delete[] fcoef;
window = get_fsamp_window(N, wintype);
float* window = get_fsamp_window(N, wintype);
switch (rtype)
{
case 0:
for (i = 0; i < N; i++)
c_impulse[i] = scale * c_impulse[2 * i] * window[i];
for (int i = 0; i < N; i++)
c_impulse[i] = (float) (scale * c_impulse[2 * i] * window[i]);
break;
case 1:
for (i = 0; i < N; i++)
for (int i = 0; i < N; i++)
{
c_impulse[2 * i + 0] *= scale * window[i];
c_impulse[2 * i + 0] *= (float) (scale * window[i]);
c_impulse[2 * i + 1] = 0.0;
}
break;
default:
break;
}
delete[] window;
return c_impulse;
}
float* FIR::fir_fsamp (int N, float* A, int rtype, double scale, int wintype)
float* FIR::fir_fsamp (int N, const float* A, int rtype, double scale, int wintype)
{
int n, i, j, k;
double sum;
float* window;
float *c_impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
auto c_impulse = new float[N * 2];
if (N & 1)
{
int M = (N - 1) / 2;
for (n = 0; n < M + 1; n++)
for (int n = 0; n < M + 1; n++)
{
sum = 0.0;
for (k = 1; k < M + 1; k++)
for (int k = 1; k < M + 1; k++)
sum += 2.0 * A[k] * cos(TWOPI * (n - M) * k / N);
c_impulse[2 * n + 0] = (1.0 / N) * (A[0] + sum);
c_impulse[2 * n + 0] = (float) ((1.0 / N) * (A[0] + sum));
c_impulse[2 * n + 1] = 0.0;
}
for (n = M + 1, j = 1; n < N; n++, j++)
for (int n = M + 1, j = 1; n < N; n++, j++)
{
c_impulse[2 * n + 0] = c_impulse[2 * (M - j) + 0];
c_impulse[2 * n + 1] = 0.0;
@ -170,34 +170,36 @@ float* FIR::fir_fsamp (int N, float* A, int rtype, double scale, int wintype)
else
{
double M = (double)(N - 1) / 2.0;
for (n = 0; n < N / 2; n++)
for (int n = 0; n < N / 2; n++)
{
sum = 0.0;
for (k = 1; k < N / 2; k++)
for (int k = 1; k < N / 2; k++)
sum += 2.0 * A[k] * cos(TWOPI * (n - M) * k / N);
c_impulse[2 * n + 0] = (1.0 / N) * (A[0] + sum);
c_impulse[2 * n + 0] = (float) ((1.0 / N) * (A[0] + sum));
c_impulse[2 * n + 1] = 0.0;
}
for (n = N / 2, j = 1; n < N; n++, j++)
for (int n = N / 2, j = 1; n < N; n++, j++)
{
c_impulse[2 * n + 0] = c_impulse[2 * (N / 2 - j) + 0];
c_impulse[2 * n + 1] = 0.0;
}
}
window = get_fsamp_window (N, wintype);
float* window = get_fsamp_window (N, wintype);
switch (rtype)
{
case 0:
for (i = 0; i < N; i++)
c_impulse[i] = scale * c_impulse[2 * i] * window[i];
for (int i = 0; i < N; i++)
c_impulse[i] = (float) (scale * c_impulse[2 * i] * window[i]);
break;
case 1:
for (i = 0; i < N; i++)
for (int i = 0; i < N; i++)
{
c_impulse[2 * i + 0] *= scale * window[i];
c_impulse[2 * i + 0] *= (float) (scale * window[i]);
c_impulse[2 * i + 1] = 0.0;
}
break;
default:
break;
}
delete[] window;
return c_impulse;
@ -205,45 +207,42 @@ float* FIR::fir_fsamp (int N, float* A, int rtype, double scale, int wintype)
float* FIR::fir_bandpass (int N, double f_low, double f_high, double samplerate, int wintype, int rtype, double scale)
{
float *c_impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
auto *c_impulse = new float[N * 2];
double ft = (f_high - f_low) / (2.0 * samplerate);
double ft_rad = TWOPI * ft;
double w_osc = PI * (f_high + f_low) / samplerate;
int i, j;
double m = 0.5 * (double)(N - 1);
double delta = PI / m;
double cosphi;
double posi, posj;
double sinc, window, coef;
double posi;
double posj;
double sinc;
double window;
double coef;
if (N & 1)
{
switch (rtype)
{
case 0:
c_impulse[N >> 1] = scale * 2.0 * ft;
c_impulse[N >> 1] = (float) (scale * 2.0 * ft);
break;
case 1:
c_impulse[N - 1] = scale * 2.0 * ft;
c_impulse[N - 1] = (float) (scale * 2.0 * ft);
c_impulse[ N ] = 0.0;
break;
default:
break;
}
}
for (i = (N + 1) / 2, j = N / 2 - 1; i < N; i++, j--)
for (int i = (N + 1) / 2, j = N / 2 - 1; i < N; i++, j--)
{
posi = (double)i - m;
posj = (double)j - m;
sinc = sin (ft_rad * posi) / (PI * posi);
switch (wintype)
if (wintype == 1) // Blackman-Harris 7-term
{
case 0: // Blackman-Harris 4-term
cosphi = cos (delta * i);
window = + 0.21747
+ cosphi * ( - 0.45325
+ cosphi * ( + 0.28256
+ cosphi * ( - 0.04672 )));
break;
case 1: // Blackman-Harris 7-term
cosphi = cos (delta * i);
window = + 6.3964424114390378e-02
+ cosphi * ( - 2.3993864599352804e-01
@ -252,20 +251,31 @@ float* FIR::fir_bandpass (int N, double f_low, double f_high, double samplerate,
+ cosphi * ( + 8.5438256055858031e-02
+ cosphi * ( - 1.2320203369293225e-02
+ cosphi * ( + 4.3778825791773474e-04 ))))));
break;
}
else // Blackman-Harris 4-term
{
cosphi = cos (delta * i);
window = + 0.21747
+ cosphi * ( - 0.45325
+ cosphi * ( + 0.28256
+ cosphi * ( - 0.04672 )));
}
coef = scale * sinc * window;
switch (rtype)
{
case 0:
c_impulse[i] = + coef * cos (posi * w_osc);
c_impulse[j] = + coef * cos (posj * w_osc);
c_impulse[i] = (float) (+ coef * cos (posi * w_osc));
c_impulse[j] = (float) (+ coef * cos (posj * w_osc));
break;
case 1:
c_impulse[2 * i + 0] = + coef * cos (posi * w_osc);
c_impulse[2 * i + 1] = - coef * sin (posi * w_osc);
c_impulse[2 * j + 0] = + coef * cos (posj * w_osc);
c_impulse[2 * j + 1] = - coef * sin (posj * w_osc);
c_impulse[2 * i + 0] = (float) (+ coef * cos (posi * w_osc));
c_impulse[2 * i + 1] = (float) (- coef * sin (posi * w_osc));
c_impulse[2 * j + 0] = (float) (+ coef * cos (posj * w_osc));
c_impulse[2 * j + 1] = (float) (- coef * sin (posj * w_osc));
break;
default:
break;
}
}
@ -282,11 +292,17 @@ float *FIR::fir_read (int N, const char *filename, int rtype, float scale)
// NOTE: The number of values in the file must NOT exceed those implied by N and rtype
{
FILE *file;
int i;
float I, Q;
float *c_impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
float I;
float Q;
auto c_impulse = new float[N * 2];
std::fill(c_impulse, c_impulse + N*2, 0);
file = fopen (filename, "r");
for (i = 0; i < N; i++)
if (!file) {
return c_impulse;
}
for (int i = 0; i < N; i++)
{
// read in the complex impulse response
// NOTE: IF the freq response is symmetrical about 0, the imag coeffs will all be zero.
@ -309,6 +325,8 @@ float *FIR::fir_read (int N, const char *filename, int rtype, float scale)
c_impulse[2 * i + 1] = - scale * Q;
break;
}
default:
break;
}
}
fclose (file);
@ -317,77 +335,74 @@ float *FIR::fir_read (int N, const char *filename, int rtype, float scale)
void FIR::analytic (int N, float* in, float* out)
{
if (N < 1) {
if (N < 2) {
return;
}
int i;
double inv_N = 1.0 / (double) N;
double two_inv_N = 2.0 * inv_N;
float* x = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
std::vector<float> x(N * 2);
fftwf_plan pfor = fftwf_plan_dft_1d (
N,
(fftwf_complex *) in,
(fftwf_complex *) x,
(fftwf_complex *) x.data(),
FFTW_FORWARD,
FFTW_PATIENT
);
fftwf_plan prev = fftwf_plan_dft_1d (
N,
(fftwf_complex *) x,
(fftwf_complex *) x.data(),
(fftwf_complex *) out,
FFTW_BACKWARD,
FFTW_PATIENT
);
fftwf_execute (pfor);
x[0] *= inv_N;
x[1] *= inv_N;
x[0] *= (float) inv_N;
x[1] *= (float) inv_N;
for (i = 1; i < N / 2; i++)
for (int i = 1; i < N / 2; i++)
{
x[2 * i + 0] *= two_inv_N;
x[2 * i + 1] *= two_inv_N;
x[2 * i + 0] *= (float) two_inv_N;
x[2 * i + 1] *= (float) two_inv_N;
}
x[N + 0] *= inv_N;
x[N + 1] *= inv_N;
x[N + 0] *= (float) inv_N;
x[N + 1] *= (float) inv_N;
memset (&x[N + 2], 0, (N - 2) * sizeof (float));
fftwf_execute (prev);
fftwf_destroy_plan (prev);
fftwf_destroy_plan (pfor);
delete[] x;
}
void FIR::mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity)
{
int i;
int size = N * pfactor;
double inv_PN = 1.0 / (float)size;
float* firpad = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
float* firfreq = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
double* mag = new double[size]; // (float *) malloc0 (size * sizeof (float));
float* ana = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
float* impulse = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
float* newfreq = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex));
std::copy(fir, fir + N * 2, firpad);
double inv_PN = 1.0 / (double)size;
std::vector<float> firpad(size * 2);
std::vector<float> firfreq(size * 2);
std::vector<double> mag(size);
std::vector<float> ana(size * 2);
std::vector<float> impulse(size * 2);
std::vector<float> newfreq(size * 2);
std::copy(fir, fir + N * 2, firpad.begin());
fftwf_plan pfor = fftwf_plan_dft_1d (
size,
(fftwf_complex *) firpad,
(fftwf_complex *) firfreq,
(fftwf_complex *) firpad.data(),
(fftwf_complex *) firfreq.data(),
FFTW_FORWARD,
FFTW_PATIENT);
fftwf_plan prev = fftwf_plan_dft_1d (
size,
(fftwf_complex *) newfreq,
(fftwf_complex *) impulse,
(fftwf_complex *) newfreq.data(),
(fftwf_complex *) impulse.data(),
FFTW_BACKWARD,
FFTW_PATIENT
);
// print_impulse("orig_imp.txt", N, fir, 1, 0);
fftwf_execute (pfor);
for (i = 0; i < size; i++)
{
@ -395,33 +410,27 @@ void FIR::mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity)
double xi = firfreq[2 * i + 1];
mag[i] = sqrt (xr*xr + xi*xi) * inv_PN;
if (mag[i] > 0.0)
ana[2 * i + 0] = log (mag[i]);
ana[2 * i + 0] = (float) log (mag[i]);
else
ana[2 * i + 0] = log (std::numeric_limits<float>::min());
}
analytic (size, ana, ana);
analytic (size, ana.data(), ana.data());
for (i = 0; i < size; i++)
{
newfreq[2 * i + 0] = + mag[i] * cos (ana[2 * i + 1]);
newfreq[2 * i + 0] = (float) (+ mag[i] * cos (ana[2 * i + 1]));
if (polarity)
newfreq[2 * i + 1] = + mag[i] * sin (ana[2 * i + 1]);
newfreq[2 * i + 1] = (float) (+ mag[i] * sin (ana[2 * i + 1]));
else
newfreq[2 * i + 1] = - mag[i] * sin (ana[2 * i + 1]);
newfreq[2 * i + 1] = (float) (- mag[i] * sin (ana[2 * i + 1]));
}
fftwf_execute (prev);
if (polarity)
std::copy(&impulse[2 * (pfactor - 1) * N], &impulse[2 * (pfactor - 1) * N] + N * 2, mpfir);
else
std::copy(impulse, impulse + N * 2, mpfir);
// print_impulse("min_imp.txt", N, mpfir, 1, 0);
std::copy(impulse.begin(), impulse.end(), mpfir);
fftwf_destroy_plan (prev);
fftwf_destroy_plan (pfor);
delete[] (newfreq);
delete[] (impulse);
delete[] (ana);
delete[] (mag);
delete[] (firfreq);
delete[] (firpad);
}
// impulse response of a zero frequency filter comprising a cascade of two resonators,
@ -432,15 +441,15 @@ float* FIR::zff_impulse(int nc, float scale)
int n_resdet = nc / 2 - 1; // size of single zero-frequency resonator with detrender
int n_dresdet = 2 * n_resdet - 1; // size of two cascaded units; when we convolve these we get 2 * n - 1 length
// allocate the single and make the values
float* resdet = new float[n_resdet]; // (float*)malloc0 (n_resdet * sizeof(float));
std::vector<float> resdet(n_resdet); // (float*)malloc0 (n_resdet * sizeof(float));
for (int i = 1, j = 0, k = n_resdet - 1; i < nc / 4; i++, j++, k--)
resdet[j] = resdet[k] = (float)(i * (i + 1) / 2);
resdet[nc / 4 - 1] = (float)(nc / 4 * (nc / 4 + 1) / 2);
// print_impulse ("resdet", n_resdet, resdet, 0, 0);
// allocate the float and complex versions and make the values
float* dresdet = new float[n_dresdet]; // (float*)malloc0 (n_dresdet * sizeof(float));
float div = (float)((nc / 2 + 1) * (nc / 2 + 1)); // calculate divisor
float* c_dresdet = new float[nc * 2]; // (float*)malloc0 (nc * sizeof(complex));
std::vector<float> dresdet(n_dresdet);
auto div = (float) ((nc / 2 + 1) * (nc / 2 + 1)); // calculate divisor
auto c_dresdet = new float[nc * 2];
for (int n = 0; n < n_dresdet; n++) // convolve to make the cascade
{
for (int k = 0; k < n_resdet; k++)
@ -450,10 +459,7 @@ float* FIR::zff_impulse(int nc, float scale)
c_dresdet[2 * n + 0] = dresdet[n] * scale;
c_dresdet[2 * n + 1] = 0.0;
}
// print_impulse("dresdet", n_dresdet, dresdet, 0, 0);
// print_impulse("c_dresdet", nc, c_dresdet, 1, 0);
delete[] (dresdet);
delete[] (resdet);
return c_dresdet;
}

4
wdsp/fir.hpp
View File

@ -35,8 +35,8 @@ class WDSP_API FIR
{
public:
static float* fftcv_mults (int NM, float* c_impulse);
static float* fir_fsamp_odd (int N, float* A, int rtype, double scale, int wintype);
static float* fir_fsamp (int N, float* A, int rtype, double scale, int wintype);
static float* fir_fsamp_odd (int N, const float* A, int rtype, double scale, int wintype);
static float* fir_fsamp (int N, const float* A, int rtype, double scale, int wintype);
static float* fir_bandpass (int N, double f_low, double f_high, double samplerate, int wintype, int rtype, double scale);
static void mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity);