VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-08-03 22:42:27 -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->dsp_size, // buffer size
rxa->midbuff, // pointer to input buffer rxa->midbuff, // pointer to input buffer
rxa->midbuff, // pointer to output buffer rxa->midbuff, // pointer to output buffer
ANF_DLINE_SIZE, // dline_size ANF::ANF_DLINE_SIZE, // dline_size
64, // taps 64, // taps
16, // delay 16, // delay
0.0001, // two_mu 0.0001, // two_mu
@ -374,7 +374,7 @@ RXA* RXA::create_rxa (
rxa->dsp_size, // buffer size rxa->dsp_size, // buffer size
rxa->midbuff, // pointer to input buffer rxa->midbuff, // pointer to input buffer
rxa->midbuff, // pointer to output buffer rxa->midbuff, // pointer to output buffer
ANR_DLINE_SIZE, // dline_size ANR::ANR_DLINE_SIZE, // dline_size
64, // taps 64, // taps
16, // delay 16, // delay
0.0001, // two_mu 0.0001, // two_mu

39
wdsp/amd.cpp
View File

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

14
wdsp/amd.hpp
View File

@ -28,15 +28,6 @@ warren@wpratt.com
#ifndef wdsp_amd_hpp #ifndef wdsp_amd_hpp
#define 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 <array>
#include "export.h" #include "export.h"
@ -61,13 +52,16 @@ public:
double phs; // pll - phase accumulator double phs; // pll - phase accumulator
double omega; // pll - locked pll frequency double omega; // pll - locked pll frequency
double fil_out; // pll - filter output 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 tauR; // carrier removal time constant
double tauI; // carrier insertion time constant double tauI; // carrier insertion time constant
double mtauR; // carrier removal multiplier double mtauR; // carrier removal multiplier
double onem_mtauR; // 1.0 - carrier_removal_multiplier double onem_mtauR; // 1.0 - carrier_removal_multiplier
double mtauI; // carrier insertion multiplier double mtauI; // carrier insertion multiplier
double onem_mtauI; // 1.0 - 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> a; // Filter a variables
std::array<double, 3*STAGES + 3> b; // Filter b variables std::array<double, 3*STAGES + 3> b; // Filter b variables
std::array<double, 3*STAGES + 3> c; // Filter c 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() void AMSQ::compute_slews()
{ {
int i; double delta;
double delta, theta; double theta;
delta = PI / (double)ntup; delta = PI / (double)ntup;
theta = 0.0; 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)); cup[i] = muted_gain + (1.0 - muted_gain) * 0.5 * (1.0 - cos (theta));
theta += delta; theta += delta;
@ -46,7 +46,7 @@ void AMSQ::compute_slews()
delta = PI / (double)ntdown; delta = PI / (double)ntdown;
theta = 0.0; 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)); cdown[i] = muted_gain + (1.0 - muted_gain) * 0.5 * (1.0 + cos (theta));
theta += delta; theta += delta;
@ -63,11 +63,11 @@ void AMSQ::calc()
// level change // level change
ntup = (int)(tup * rate); ntup = (int)(tup * rate);
ntdown = (int)(tdown * rate); ntdown = (int)(tdown * rate);
cup.resize((ntup + 1) * 2); // (float *)malloc0((ntup + 1) * sizeof(float)); cup.resize((ntup + 1) * 2);
cdown.resize((ntdown + 1) * 2); // (float *)malloc0((ntdown + 1) * sizeof(float)); cdown.resize((ntdown + 1) * 2);
compute_slews(); compute_slews();
// control // control
state = 0; state = AMSQState::MUTED;
} }
AMSQ::AMSQ ( AMSQ::AMSQ (
@ -108,26 +108,17 @@ void AMSQ::flush()
{ {
std::fill(trigsig.begin(), trigsig.end(), 0); std::fill(trigsig.begin(), trigsig.end(), 0);
avsig = 0.0; avsig = 0.0;
state = 0; state = AMSQState::MUTED;
} }
enum _amsqstate
{
MUTED,
INCREASE,
UNMUTED,
TAIL,
DECREASE
};
void AMSQ::execute() void AMSQ::execute()
{ {
if (run) if (run)
{ {
int i; double sig;
double sig, siglimit; double siglimit;
for (i = 0; i < size; i++) for (int i = 0; i < size; i++)
{ {
double trigr = trigsig[2 * i + 0]; double trigr = trigsig[2 * i + 0];
double trigi = trigsig[2 * i + 1]; double trigi = trigsig[2 * i + 1];
@ -136,31 +127,31 @@ void AMSQ::execute()
switch (state) switch (state)
{ {
case MUTED: case AMSQState::MUTED:
if (avsig > unmute_thresh) if (avsig > unmute_thresh)
{ {
state = INCREASE; state = AMSQState::INCREASE;
count = ntup; count = ntup;
} }
out[2 * i + 0] = muted_gain * in[2 * i + 0]; out[2 * i + 0] = (float) (muted_gain * in[2 * i + 0]);
out[2 * i + 1] = muted_gain * in[2 * i + 1]; out[2 * i + 1] = (float) (muted_gain * in[2 * i + 1]);
break; break;
case INCREASE: case AMSQState::INCREASE:
out[2 * i + 0] = in[2 * i + 0] * cup[ntup - count]; out[2 * i + 0] = (float) (in[2 * i + 0] * cup[ntup - count]);
out[2 * i + 1] = in[2 * i + 1] * cup[ntup - count]; out[2 * i + 1] = (float) (in[2 * i + 1] * cup[ntup - count]);
if (count-- == 0) if (count-- == 0)
state = UNMUTED; state = AMSQState::UNMUTED;
break; break;
case UNMUTED: case AMSQState::UNMUTED:
if (avsig < tail_thresh) if (avsig < tail_thresh)
{ {
state = TAIL; state = AMSQState::TAIL;
if ((siglimit = avsig) > 1.0) if ((siglimit = avsig) > 1.0)
siglimit = 1.0; siglimit = 1.0;
@ -173,28 +164,28 @@ void AMSQ::execute()
break; break;
case TAIL: case AMSQState::TAIL:
out[2 * i + 0] = in[2 * i + 0]; out[2 * i + 0] = in[2 * i + 0];
out[2 * i + 1] = in[2 * i + 1]; out[2 * i + 1] = in[2 * i + 1];
if (avsig > unmute_thresh) if (avsig > unmute_thresh)
{ {
state = UNMUTED; state = AMSQState::UNMUTED;
} }
else if (count-- == 0) else if (count-- == 0)
{ {
state = DECREASE; state = AMSQState::TAIL;
count = ntdown; count = ntdown;
} }
break; break;
case DECREASE: case AMSQState::DECREASE:
out[2 * i + 0] = in[2 * i + 0] * cdown[ntdown - count]; out[2 * i + 0] = (float) (in[2 * i + 0] * cdown[ntdown - count]);
out[2 * i + 1] = in[2 * i + 1] * cdown[ntdown - count]; out[2 * i + 1] = (float) (in[2 * i + 1] * cdown[ntdown - count]);
if (count-- == 0) if (count-- == 0)
state = MUTED; state = AMSQState::MUTED;
break; break;
} }

11
wdsp/amsq.hpp
View File

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

55
wdsp/anb.cpp
View File

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

60
wdsp/anf.cpp
View File

@ -53,49 +53,53 @@ ANF::ANF(
double _den_mult, double _den_mult,
double _lincr, double _lincr,
double _ldecr 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; 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(d.begin(), d.end(), 0);
std::fill(w.begin(), w.end(), 0); std::fill(w.begin(), w.end(), 0);
} }
void ANF::execute(int _position) void ANF::execute(int _position)
{ {
int i, j, idx; int idx;
double c0, c1; double c0;
double y, error, sigma, inv_sigp; double c1;
double nel, nev; double y;
double error;
double sigma;
double inv_sigp;
double nel;
double nev;
if (run && (position == _position)) 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]; d[in_idx] = in_buff[2 * i + 0];
y = 0; y = 0;
sigma = 0; sigma = 0;
for (j = 0; j < n_taps; j++) for (int j = 0; j < n_taps; j++)
{ {
idx = (in_idx + j + delay) & mask; idx = (in_idx + j + delay) & mask;
y += w[j] * d[idx]; y += w[j] * d[idx];
@ -105,7 +109,7 @@ void ANF::execute(int _position)
inv_sigp = 1.0 / (sigma + 1e-10); inv_sigp = 1.0 / (sigma + 1e-10);
error = d[in_idx] - y; 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; out_buff[2 * i + 1] = 0.0;
if ((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 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; c0 = 1.0 - two_mu * ngamma;
c1 = two_mu * error * inv_sigp; 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; idx = (in_idx + j + delay) & mask;
w[j] = c0 * w[j] + c1 * d[idx]; w[j] = c0 * w[j] + c1 * d[idx];

3
wdsp/anf.hpp
View File

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

21
wdsp/anr.cpp
View File

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

3
wdsp/anr.hpp
View File

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

72
wdsp/bandpass.cpp
View File

@ -51,21 +51,21 @@ BANDPASS::BANDPASS(
int _samplerate, int _samplerate,
int _wintype, int _wintype,
double _gain double _gain
) ) :
{
// NOTE: 'nc' must be >= 'size' // NOTE: 'nc' must be >= 'size'
run = _run; run(_run),
position = _position; position(_position),
size = _size; size(_size),
nc = _nc; nc(_nc),
mp = _mp; mp(_mp),
in = _in; in(_in),
out = _out; out(_out),
f_low = _f_low; f_low(_f_low),
f_high = _f_high; f_high(_f_high),
samplerate = _samplerate; samplerate(_samplerate),
wintype = _wintype; wintype(_wintype),
gain = _gain; gain(_gain)
{
float* impulse = FIR::fir_bandpass ( float* impulse = FIR::fir_bandpass (
nc, nc,
f_low, f_low,
@ -136,7 +136,7 @@ void BANDPASS::setSize(int _size)
gain / (double) (2 * size) gain / (double) (2 * size)
); );
FIRCORE::setImpulse_fircore (fircore, impulse, 1); FIRCORE::setImpulse_fircore (fircore, impulse, 1);
delete[] (impulse); delete[] impulse;
} }
void BANDPASS::setGain(double _gain, int _update) void BANDPASS::setGain(double _gain, int _update)
@ -152,7 +152,7 @@ void BANDPASS::setGain(double _gain, int _update)
gain / (double) (2 * size) gain / (double) (2 * size)
); );
FIRCORE::setImpulse_fircore (fircore, impulse, _update); FIRCORE::setImpulse_fircore (fircore, impulse, _update);
delete[] (impulse); delete[] impulse;
} }
void BANDPASS::calcBandpassFilter(double _f_low, double _f_high, double _gain) 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) gain / (double)(2 * size)
); );
FIRCORE::setImpulse_fircore (fircore, impulse, 1); 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); FIRCORE::setImpulse_fircore (fircore, impulse, 0);
delete[] (impulse); delete[] impulse;
f_low = _f_low; f_low = _f_low;
f_high = _f_high; f_high = _f_high;
FIRCORE::setUpdate_fircore (fircore); FIRCORE::setUpdate_fircore (fircore);
@ -220,7 +220,7 @@ void BANDPASS::SetBandpassNC(int _nc)
gain / (double)( 2 * size) gain / (double)( 2 * size)
); );
FIRCORE::setNc_fircore (fircore, nc, impulse); 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 } // namespace WDSP

11
wdsp/bpsnba.cpp
View File

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

7
wdsp/bpsnba.hpp
View File

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

16
wdsp/cblock.cpp
View File

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

2
wdsp/dsphp.cpp
View File

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

9
wdsp/dsphp.hpp
View File

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

316
wdsp/emnr.cpp
View File

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

13
wdsp/emnr.hpp
View File

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

250
wdsp/fir.cpp
View File

@ -27,6 +27,7 @@ warren@pratt.one
#define _CRT_SECURE_NO_WARNINGS #define _CRT_SECURE_NO_WARNINGS
#include <limits> #include <limits>
#include <vector>
#include "fftw3.h" #include "fftw3.h"
#include "comm.hpp" #include "comm.hpp"
@ -36,132 +37,131 @@ namespace WDSP {
float* FIR::fftcv_mults (int NM, float* c_impulse) float* FIR::fftcv_mults (int NM, float* c_impulse)
{ {
float* mults = new float[NM * 2]; auto mults = new float[NM * 2];
float* cfft_impulse = new float[NM * 2]; std::vector<float> cfft_impulse(NM * 2);
fftwf_plan ptmp = fftwf_plan_dft_1d( fftwf_plan ptmp = fftwf_plan_dft_1d(
NM, NM,
(fftwf_complex *) cfft_impulse, (fftwf_complex *) cfft_impulse.data(),
(fftwf_complex *) mults, (fftwf_complex *) mults,
FFTW_FORWARD, FFTW_FORWARD,
FFTW_PATIENT 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 // store complex coefs right-justified in the buffer
std::copy(c_impulse, c_impulse + (NM / 2 + 1) * 2, &(cfft_impulse[NM - 2])); std::copy(c_impulse, c_impulse + (NM / 2 + 1) * 2, &(cfft_impulse[NM - 2]));
fftwf_execute (ptmp); fftwf_execute (ptmp);
fftwf_destroy_plan (ptmp); fftwf_destroy_plan (ptmp);
delete[] cfft_impulse;
return mults; return mults;
} }
float* FIR::get_fsamp_window(int N, int wintype) float* FIR::get_fsamp_window(int N, int wintype)
{ {
int i; double arg0;
double arg0, arg1; double arg1;
float* window = new float[N]; // (float *) malloc0 (N * sizeof(float)); auto window = new float[N];
switch (wintype) switch (wintype)
{ {
case 0: case 0:
arg0 = 2.0 * PI / ((double)N - 1.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); arg1 = cos(arg0 * (double)i);
window[i] = +0.21747 double val = +0.21747
+ arg1 * (-0.45325 + arg1 * (-0.45325
+ arg1 * (+0.28256 + arg1 * (+0.28256
+ arg1 * (-0.04672))); + arg1 * (-0.04672)));
window[i] = (float) val;
} }
break; break;
case 1: case 1:
arg0 = 2.0 * PI / ((double)N - 1.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); arg1 = cos(arg0 * (double)i);
window[i] = +6.3964424114390378e-02 double val = +6.3964424114390378e-02
+ arg1 * (-2.3993864599352804e-01 + arg1 * (-2.3993864599352804e-01
+ arg1 * (+3.5015956323820469e-01 + arg1 * (+3.5015956323820469e-01
+ arg1 * (-2.4774111897080783e-01 + arg1 * (-2.4774111897080783e-01
+ arg1 * (+8.5438256055858031e-02 + arg1 * (+8.5438256055858031e-02
+ arg1 * (-1.2320203369293225e-02 + arg1 * (-1.2320203369293225e-02
+ arg1 * (+4.3778825791773474e-04)))))); + arg1 * (+4.3778825791773474e-04))))));
window[i] = (float) val;
} }
break; break;
default: default:
for (i = 0; i < N; i++) for (int i = 0; i < N; i++)
window[i] = 1.0; window[i] = 1.0;
} }
return window; 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; int mid = (N - 1) / 2;
double mag, phs; double mag;
float* window; double phs;
float *fcoef = new float[N * 2]; std::vector<float> fcoef(N * 2);
float *c_impulse = new float[N * 2]; auto *c_impulse = new float[N * 2];
fftwf_plan ptmp = fftwf_plan_dft_1d( fftwf_plan ptmp = fftwf_plan_dft_1d(
N, N,
(fftwf_complex *)fcoef, (fftwf_complex *)fcoef.data(),
(fftwf_complex *)c_impulse, (fftwf_complex *)c_impulse,
FFTW_BACKWARD, FFTW_BACKWARD,
FFTW_PATIENT FFTW_PATIENT
); );
double local_scale = 1.0 / (double) N; 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; mag = A[i] * local_scale;
phs = - (double)mid * TWOPI * (double)i / (double)N; phs = - (double)mid * TWOPI * (double)i / (double)N;
fcoef[2 * i + 0] = mag * cos (phs); fcoef[2 * i + 0] = (float) (mag * cos (phs));
fcoef[2 * i + 1] = mag * sin (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 + 0] = + fcoef[2 * (mid - j) + 0];
fcoef[2 * i + 1] = - fcoef[2 * (mid - j) + 1]; fcoef[2 * i + 1] = - fcoef[2 * (mid - j) + 1];
} }
fftwf_execute (ptmp); fftwf_execute (ptmp);
fftwf_destroy_plan (ptmp); fftwf_destroy_plan (ptmp);
delete[] fcoef; float* window = get_fsamp_window(N, wintype);
window = get_fsamp_window(N, wintype);
switch (rtype) switch (rtype)
{ {
case 0: case 0:
for (i = 0; i < N; i++) for (int i = 0; i < N; i++)
c_impulse[i] = scale * c_impulse[2 * i] * window[i]; c_impulse[i] = (float) (scale * c_impulse[2 * i] * window[i]);
break; break;
case 1: 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; c_impulse[2 * i + 1] = 0.0;
} }
break; break;
default:
break;
} }
delete[] window; delete[] window;
return c_impulse; 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; double sum;
float* window; auto c_impulse = new float[N * 2];
float *c_impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex));
if (N & 1) if (N & 1)
{ {
int M = (N - 1) / 2; int M = (N - 1) / 2;
for (n = 0; n < M + 1; n++) for (int n = 0; n < M + 1; n++)
{ {
sum = 0.0; 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); 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; 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 + 0] = c_impulse[2 * (M - j) + 0];
c_impulse[2 * n + 1] = 0.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 else
{ {
double M = (double)(N - 1) / 2.0; 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; 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); 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; 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 + 0] = c_impulse[2 * (N / 2 - j) + 0];
c_impulse[2 * n + 1] = 0.0; c_impulse[2 * n + 1] = 0.0;
} }
} }
window = get_fsamp_window (N, wintype); float* window = get_fsamp_window (N, wintype);
switch (rtype) switch (rtype)
{ {
case 0: case 0:
for (i = 0; i < N; i++) for (int i = 0; i < N; i++)
c_impulse[i] = scale * c_impulse[2 * i] * window[i]; c_impulse[i] = (float) (scale * c_impulse[2 * i] * window[i]);
break; break;
case 1: 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; c_impulse[2 * i + 1] = 0.0;
} }
break; break;
default:
break;
} }
delete[] window; delete[] window;
return c_impulse; 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* 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 = (f_high - f_low) / (2.0 * samplerate);
double ft_rad = TWOPI * ft; double ft_rad = TWOPI * ft;
double w_osc = PI * (f_high + f_low) / samplerate; double w_osc = PI * (f_high + f_low) / samplerate;
int i, j;
double m = 0.5 * (double)(N - 1); double m = 0.5 * (double)(N - 1);
double delta = PI / m; double delta = PI / m;
double cosphi; double cosphi;
double posi, posj; double posi;
double sinc, window, coef; double posj;
double sinc;
double window;
double coef;
if (N & 1) if (N & 1)
{ {
switch (rtype) switch (rtype)
{ {
case 0: case 0:
c_impulse[N >> 1] = scale * 2.0 * ft; c_impulse[N >> 1] = (float) (scale * 2.0 * ft);
break; break;
case 1: case 1:
c_impulse[N - 1] = scale * 2.0 * ft; c_impulse[N - 1] = (float) (scale * 2.0 * ft);
c_impulse[ N ] = 0.0; c_impulse[ N ] = 0.0;
break; 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; posi = (double)i - m;
posj = (double)j - m; posj = (double)j - m;
sinc = sin (ft_rad * posi) / (PI * posi); 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); cosphi = cos (delta * i);
window = + 6.3964424114390378e-02 window = + 6.3964424114390378e-02
+ cosphi * ( - 2.3993864599352804e-01 + 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 * ( + 8.5438256055858031e-02
+ cosphi * ( - 1.2320203369293225e-02 + cosphi * ( - 1.2320203369293225e-02
+ cosphi * ( + 4.3778825791773474e-04 )))))); + 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; coef = scale * sinc * window;
switch (rtype) switch (rtype)
{ {
case 0: case 0:
c_impulse[i] = + coef * cos (posi * w_osc); c_impulse[i] = (float) (+ coef * cos (posi * w_osc));
c_impulse[j] = + coef * cos (posj * w_osc); c_impulse[j] = (float) (+ coef * cos (posj * w_osc));
break; break;
case 1: case 1:
c_impulse[2 * i + 0] = + coef * cos (posi * w_osc); c_impulse[2 * i + 0] = (float) (+ coef * cos (posi * w_osc));
c_impulse[2 * i + 1] = - coef * sin (posi * w_osc); c_impulse[2 * i + 1] = (float) (- coef * sin (posi * w_osc));
c_impulse[2 * j + 0] = + coef * cos (posj * w_osc); c_impulse[2 * j + 0] = (float) (+ coef * cos (posj * w_osc));
c_impulse[2 * j + 1] = - coef * sin (posj * w_osc); c_impulse[2 * j + 1] = (float) (- coef * sin (posj * w_osc));
break;
default:
break; 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 // NOTE: The number of values in the file must NOT exceed those implied by N and rtype
{ {
FILE *file; FILE *file;
int i; float I;
float I, Q; float Q;
float *c_impulse = new float[N * 2]; // (float *) malloc0 (N * sizeof (complex)); auto c_impulse = new float[N * 2];
std::fill(c_impulse, c_impulse + N*2, 0);
file = fopen (filename, "r"); 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 // read in the complex impulse response
// NOTE: IF the freq response is symmetrical about 0, the imag coeffs will all be zero. // 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; c_impulse[2 * i + 1] = - scale * Q;
break; break;
} }
default:
break;
} }
} }
fclose (file); 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) void FIR::analytic (int N, float* in, float* out)
{ {
if (N < 1) { if (N < 2) {
return; return;
} }
int i;
double inv_N = 1.0 / (double) N; double inv_N = 1.0 / (double) N;
double two_inv_N = 2.0 * inv_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 ( fftwf_plan pfor = fftwf_plan_dft_1d (
N, N,
(fftwf_complex *) in, (fftwf_complex *) in,
(fftwf_complex *) x, (fftwf_complex *) x.data(),
FFTW_FORWARD, FFTW_FORWARD,
FFTW_PATIENT FFTW_PATIENT
); );
fftwf_plan prev = fftwf_plan_dft_1d ( fftwf_plan prev = fftwf_plan_dft_1d (
N, N,
(fftwf_complex *) x, (fftwf_complex *) x.data(),
(fftwf_complex *) out, (fftwf_complex *) out,
FFTW_BACKWARD, FFTW_BACKWARD,
FFTW_PATIENT FFTW_PATIENT
); );
fftwf_execute (pfor); fftwf_execute (pfor);
x[0] *= inv_N; x[0] *= (float) inv_N;
x[1] *= 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 + 0] *= (float) two_inv_N;
x[2 * i + 1] *= two_inv_N; x[2 * i + 1] *= (float) two_inv_N;
} }
x[N + 0] *= inv_N; x[N + 0] *= (float) inv_N;
x[N + 1] *= inv_N; x[N + 1] *= (float) inv_N;
memset (&x[N + 2], 0, (N - 2) * sizeof (float)); memset (&x[N + 2], 0, (N - 2) * sizeof (float));
fftwf_execute (prev); fftwf_execute (prev);
fftwf_destroy_plan (prev); fftwf_destroy_plan (prev);
fftwf_destroy_plan (pfor); fftwf_destroy_plan (pfor);
delete[] x;
} }
void FIR::mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity) void FIR::mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity)
{ {
int i; int i;
int size = N * pfactor; int size = N * pfactor;
double inv_PN = 1.0 / (float)size; double inv_PN = 1.0 / (double)size;
float* firpad = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex)); std::vector<float> firpad(size * 2);
float* firfreq = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex)); std::vector<float> firfreq(size * 2);
double* mag = new double[size]; // (float *) malloc0 (size * sizeof (float)); std::vector<double> mag(size);
float* ana = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex)); std::vector<float> ana(size * 2);
float* impulse = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex)); std::vector<float> impulse(size * 2);
float* newfreq = new float[size * 2]; // (float *) malloc0 (size * sizeof (complex)); std::vector<float> newfreq(size * 2);
std::copy(fir, fir + N * 2, firpad); std::copy(fir, fir + N * 2, firpad.begin());
fftwf_plan pfor = fftwf_plan_dft_1d ( fftwf_plan pfor = fftwf_plan_dft_1d (
size, size,
(fftwf_complex *) firpad, (fftwf_complex *) firpad.data(),
(fftwf_complex *) firfreq, (fftwf_complex *) firfreq.data(),
FFTW_FORWARD, FFTW_FORWARD,
FFTW_PATIENT); FFTW_PATIENT);
fftwf_plan prev = fftwf_plan_dft_1d ( fftwf_plan prev = fftwf_plan_dft_1d (
size, size,
(fftwf_complex *) newfreq, (fftwf_complex *) newfreq.data(),
(fftwf_complex *) impulse, (fftwf_complex *) impulse.data(),
FFTW_BACKWARD, FFTW_BACKWARD,
FFTW_PATIENT FFTW_PATIENT
); );
// print_impulse("orig_imp.txt", N, fir, 1, 0);
fftwf_execute (pfor); fftwf_execute (pfor);
for (i = 0; i < size; i++) 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]; double xi = firfreq[2 * i + 1];
mag[i] = sqrt (xr*xr + xi*xi) * inv_PN; mag[i] = sqrt (xr*xr + xi*xi) * inv_PN;
if (mag[i] > 0.0) if (mag[i] > 0.0)
ana[2 * i + 0] = log (mag[i]); ana[2 * i + 0] = (float) log (mag[i]);
else else
ana[2 * i + 0] = log (std::numeric_limits<float>::min()); 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++) 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) 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 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); fftwf_execute (prev);
if (polarity) if (polarity)
std::copy(&impulse[2 * (pfactor - 1) * N], &impulse[2 * (pfactor - 1) * N] + N * 2, mpfir); std::copy(&impulse[2 * (pfactor - 1) * N], &impulse[2 * (pfactor - 1) * N] + N * 2, mpfir);
else else
std::copy(impulse, impulse + N * 2, mpfir); std::copy(impulse.begin(), impulse.end(), mpfir);
// print_impulse("min_imp.txt", N, mpfir, 1, 0);
fftwf_destroy_plan (prev); fftwf_destroy_plan (prev);
fftwf_destroy_plan (pfor); 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, // 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_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 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 // 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--) 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[j] = resdet[k] = (float)(i * (i + 1) / 2);
resdet[nc / 4 - 1] = (float)(nc / 4 * (nc / 4 + 1) / 2); resdet[nc / 4 - 1] = (float)(nc / 4 * (nc / 4 + 1) / 2);
// print_impulse ("resdet", n_resdet, resdet, 0, 0); // print_impulse ("resdet", n_resdet, resdet, 0, 0);
// allocate the float and complex versions and make the values // allocate the float and complex versions and make the values
float* dresdet = new float[n_dresdet]; // (float*)malloc0 (n_dresdet * sizeof(float)); std::vector<float> dresdet(n_dresdet);
float div = (float)((nc / 2 + 1) * (nc / 2 + 1)); // calculate divisor auto div = (float) ((nc / 2 + 1) * (nc / 2 + 1)); // calculate divisor
float* c_dresdet = new float[nc * 2]; // (float*)malloc0 (nc * sizeof(complex)); auto c_dresdet = new float[nc * 2];
for (int n = 0; n < n_dresdet; n++) // convolve to make the cascade for (int n = 0; n < n_dresdet; n++) // convolve to make the cascade
{ {
for (int k = 0; k < n_resdet; k++) 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 + 0] = dresdet[n] * scale;
c_dresdet[2 * n + 1] = 0.0; 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; return c_dresdet;
} }

4
wdsp/fir.hpp
View File

@ -35,8 +35,8 @@ class WDSP_API FIR
{ {
public: public:
static float* fftcv_mults (int NM, float* c_impulse); 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_odd (int N, const 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 (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 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); static void mp_imp (int N, float* fir, float* mpfir, int pfactor, int polarity);