From b9e3b10a6b76732173910b48d41dfada7786ea93 Mon Sep 17 00:00:00 2001 From: f4exb Date: Wed, 31 Jul 2024 01:37:17 +0200 Subject: [PATCH] WDSP: split iir source files --- plugins/channelrx/wdsprx/wdsprxsink.cpp | 4 +- wdsp/CMakeLists.txt | 24 +- wdsp/RXA.cpp | 3 +- wdsp/TXA.cpp | 2 +- wdsp/TXA.hpp | 1 - wdsp/bqbp.cpp | 159 +++ wdsp/bqbp.hpp | 70 ++ wdsp/bqlp.cpp | 155 +++ wdsp/bqlp.hpp | 70 ++ wdsp/dbqbp.cpp | 155 +++ wdsp/dbqbp.hpp | 71 ++ wdsp/dbqlp.cpp | 151 +++ wdsp/dbqlp.hpp | 70 ++ wdsp/dsphp.cpp | 140 +++ wdsp/dsphp.hpp | 71 ++ wdsp/fmd.cpp | 2 +- wdsp/iir.cpp | 1387 ----------------------- wdsp/iir.hpp | 440 ------- wdsp/mpeak.cpp | 221 ++++ wdsp/mpeak.hpp | 96 ++ wdsp/phrot.cpp | 182 +++ wdsp/phrot.hpp | 78 ++ wdsp/snotch.cpp | 136 +++ wdsp/snotch.hpp | 70 ++ wdsp/speak.cpp | 260 +++++ wdsp/speak.hpp | 89 ++ wdsp/sphp.cpp | 141 +++ wdsp/sphp.hpp | 72 ++ wdsp/ssql.cpp | 2 +- wdsp/ssql.hpp | 4 +- 30 files changed, 2488 insertions(+), 1838 deletions(-) create mode 100644 wdsp/bqbp.cpp create mode 100644 wdsp/bqbp.hpp create mode 100644 wdsp/bqlp.cpp create mode 100644 wdsp/bqlp.hpp create mode 100644 wdsp/dbqbp.cpp create mode 100644 wdsp/dbqbp.hpp create mode 100644 wdsp/dbqlp.cpp create mode 100644 wdsp/dbqlp.hpp create mode 100644 wdsp/dsphp.cpp create mode 100644 wdsp/dsphp.hpp delete mode 100644 wdsp/iir.cpp delete mode 100644 wdsp/iir.hpp create mode 100644 wdsp/mpeak.cpp create mode 100644 wdsp/mpeak.hpp create mode 100644 wdsp/phrot.cpp create mode 100644 wdsp/phrot.hpp create mode 100644 wdsp/snotch.cpp create mode 100644 wdsp/snotch.hpp create mode 100644 wdsp/speak.cpp create mode 100644 wdsp/speak.hpp create mode 100644 wdsp/sphp.cpp create mode 100644 wdsp/sphp.hpp diff --git a/plugins/channelrx/wdsprx/wdsprxsink.cpp b/plugins/channelrx/wdsprx/wdsprxsink.cpp index c6e7593e9..9edf7c2b5 100644 --- a/plugins/channelrx/wdsprx/wdsprxsink.cpp +++ b/plugins/channelrx/wdsprx/wdsprxsink.cpp @@ -38,12 +38,12 @@ #include "nob.hpp" #include "amd.hpp" #include "fmd.hpp" -#include "iir.cpp" #include "ssql.hpp" #include "amsq.hpp" #include "fmsq.hpp" -#include "eq.hpp" +#include "eqp.hpp" #include "shift.hpp" +#include "speak.hpp" #include "wdsprxsink.h" diff --git a/wdsp/CMakeLists.txt b/wdsp/CMakeLists.txt index abb56e609..fea3b3a1c 100644 --- a/wdsp/CMakeLists.txt +++ b/wdsp/CMakeLists.txt @@ -12,12 +12,17 @@ set(wdsp_SOURCES bldr.cpp bps.cpp bpsnba.cpp + bqbp.cpp + bqlp.cpp calculus.cpp cblock.cpp cfcomp.cpp cfir.cpp compress.cpp + dbqbp.cpp + dbqlp.cpp delay.cpp + dsphp.cpp emnr.cpp emph.cpp eqp.cpp @@ -32,15 +37,17 @@ set(wdsp_SOURCES gain.cpp gen.cpp icfir.cpp - iir.cpp + # iir.cpp iqc.cpp lmath.cpp meter.cpp meterlog10.cpp + mpeak.cpp nbp.cpp nob.cpp osctrl.cpp patchpanel.cpp + phrot.cpp resample.cpp resamplef.cpp rmatch.cpp @@ -50,6 +57,9 @@ set(wdsp_SOURCES siphon.cpp slew.cpp snba.cpp + snotch.cpp + speak.cpp + sphp.cpp ssql.cpp TXA.cpp varsamp.cpp @@ -67,6 +77,8 @@ set(wdsp_HEADERS bldr.hpp bps.hpp bpsnba.hpp + bqbp.hpp + bqlp.hpp bufferprobe.hpp calculus.hpp cblock.hpp @@ -74,7 +86,10 @@ set(wdsp_HEADERS cfir.hpp comm.hpp compress.hpp + dbqbp.hpp + dbqlp.hpp delay.hpp + dsphp.hpp emnr.hpp emph.hpp eqp.hpp @@ -89,15 +104,17 @@ set(wdsp_HEADERS gain.hpp gen.hpp icfir.hpp - iir.hpp + # iir.hpp iqc.hpp lmath.hpp meter.hpp meterlog10.hpp + mpeak.hpp nbp.hpp nob.hpp osctrl.hpp patchpanel.hpp + phrot.hpp resample.hpp resamplef.hpp rmatch.hpp @@ -107,6 +124,9 @@ set(wdsp_HEADERS siphon.hpp slew.hpp snba.hpp + snotch.hpp + speak.hpp + sphp.hpp ssql.hpp TXA.hpp varsamp.hpp diff --git a/wdsp/RXA.cpp b/wdsp/RXA.cpp index 289e27091..b898722e5 100644 --- a/wdsp/RXA.cpp +++ b/wdsp/RXA.cpp @@ -49,11 +49,12 @@ warren@wpratt.com #include "siphon.hpp" #include "cblock.hpp" #include "ssql.hpp" -#include "iir.hpp" #include "fircore.hpp" #include "wcpAGC.hpp" #include "anb.hpp" #include "nob.hpp" +#include "speak.hpp" +#include "mpeak.hpp" namespace WDSP { diff --git a/wdsp/TXA.cpp b/wdsp/TXA.cpp index 03d637453..84191334f 100644 --- a/wdsp/TXA.cpp +++ b/wdsp/TXA.cpp @@ -33,7 +33,6 @@ warren@wpratt.com #include "patchpanel.hpp" #include "amsq.hpp" #include "eq.hpp" -#include "iir.hpp" #include "cfcomp.hpp" #include "compress.hpp" #include "bandpass.hpp" @@ -48,6 +47,7 @@ warren@wpratt.com #include "iqc.hpp" #include "cfir.hpp" #include "fircore.hpp" +#include "phrot.hpp" #include "fir.hpp" #include "TXA.hpp" diff --git a/wdsp/TXA.hpp b/wdsp/TXA.hpp index c7b5fa203..284f2a643 100644 --- a/wdsp/TXA.hpp +++ b/wdsp/TXA.hpp @@ -37,7 +37,6 @@ warren@wpratt.com #include "patchpanel.hpp" #include "amsq.hpp" #include "eqp.hpp" -#include "iir.hpp" #include "cfcomp.hpp" #include "compress.hpp" #include "bandpass.hpp" diff --git a/wdsp/bqbp.cpp b/wdsp/bqbp.cpp new file mode 100644 index 000000000..0026fb004 --- /dev/null +++ b/wdsp/bqbp.cpp @@ -0,0 +1,159 @@ +/* iir.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "bqbp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Complex Bi-Quad Band-Pass * +* * +********************************************************************************************************/ + +void BQBP::calc_bqbp(BQBP *a) +{ + double f0, w0, bw, q, sn, cs, c, den; + bw = a->f_high - a->f_low; + f0 = (a->f_high + a->f_low) / 2.0; + q = f0 / bw; + w0 = TWOPI * f0 / a->rate; + sn = sin(w0); + cs = cos(w0); + c = sn / (2.0 * q); + den = 1.0 + c; + a->a0 = +c / den; + a->a1 = 0.0; + a->a2 = -c / den; + a->b1 = 2.0 * cs / den; + a->b2 = (c - 1.0) / den; + flush_bqbp(a); +} + +BQBP* BQBP::create_bqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages) +{ + BQBP *a = new BQBP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->f_low = f_low; + a->f_high = f_high; + a->gain = gain; + a->nstages = nstages; + a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->x2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + calc_bqbp(a); + return a; +} + +void BQBP::destroy_bqbp(BQBP *a) +{ + delete[](a->y2); + delete[](a->y1); + delete[](a->y0); + delete[](a->x2); + delete[](a->x1); + delete[](a->x0); + delete(a); +} + +void BQBP::flush_bqbp(BQBP *a) +{ + int i; + for (i = 0; i < a->nstages; i++) + { + a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; + a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; + } +} + +void BQBP::xbqbp(BQBP *a) +{ + if (a->run) + { + int i, j, n; + + for (i = 0; i < a->size; i++) + { + for (j = 0; j < 2; j++) + { + a->x0[j] = a->gain * a->in[2 * i + j]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; + + a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] + + a->a1 * a->x1[2 * n + j] + + a->a2 * a->x2[2 * n + j] + + a->b1 * a->y1[2 * n + j] + + a->b2 * a->y2[2 * n + j]; + a->y2[2 * n + j] = a->y1[2 * n + j]; + a->y1[2 * n + j] = a->y0[2 * n + j]; + a->x2[2 * n + j] = a->x1[2 * n + j]; + a->x1[2 * n + j] = a->x0[2 * n + j]; + } + + a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; + } + } + } + else if (a->out != a->in) + { + std::copy(a->in, a->in + a->size * 2, a->out); + } +} + +void BQBP::setBuffers_bqbp(BQBP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void BQBP::setSamplerate_bqbp(BQBP *a, int rate) +{ + a->rate = rate; + calc_bqbp(a); +} + +void BQBP::setSize_bqbp(BQBP *a, int size) +{ + a->size = size; + flush_bqbp(a); +} + +} // namespace WDSP diff --git a/wdsp/bqbp.hpp b/wdsp/bqbp.hpp new file mode 100644 index 000000000..a530e552a --- /dev/null +++ b/wdsp/bqbp.hpp @@ -0,0 +1,70 @@ +/* bqbp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Complex Bi-Quad Band-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_bqbp_h +#define wdsp_bqbp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API BQBP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double f_low; + double f_high; + double gain; + int nstages; + double a0, a1, a2, b1, b2; + double* x0, * x1, * x2, * y0, * y1, * y2; + + static BQBP* create_bqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages); + static void destroy_bqbp(BQBP *a); + static void flush_bqbp(BQBP *a); + static void xbqbp(BQBP *a); + static void setBuffers_bqbp(BQBP *a, float* in, float* out); + static void setSamplerate_bqbp(BQBP *a, int rate); + static void setSize_bqbp(BQBP *a, int size); + +private: + static void calc_bqbp(BQBP *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/bqlp.cpp b/wdsp/bqlp.cpp new file mode 100644 index 000000000..c18caae4f --- /dev/null +++ b/wdsp/bqlp.cpp @@ -0,0 +1,155 @@ +/* iir.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "bqlp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Complex Bi-Quad Low-Pass * +* * +********************************************************************************************************/ + +void BQLP::calc_bqlp(BQLP *a) +{ + double w0, cs, c, den; + w0 = TWOPI * a->fc / (double)a->rate; + cs = cos(w0); + c = sin(w0) / (2.0 * a->Q); + den = 1.0 + c; + a->a0 = 0.5 * (1.0 - cs) / den; + a->a1 = (1.0 - cs) / den; + a->a2 = 0.5 * (1.0 - cs) / den; + a->b1 = 2.0 * cs / den; + a->b2 = (c - 1.0) / den; + flush_bqlp(a); +} + +BQLP* BQLP::create_bqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages) +{ + BQLP *a = new BQLP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->fc = fc; + a->Q = Q; + a->gain = gain; + a->nstages = nstages; + a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->x2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + calc_bqlp(a); + return a; +} + +void BQLP::destroy_bqlp(BQLP *a) +{ + delete[](a->y2); + delete[](a->y1); + delete[](a->y0); + delete[](a->x2); + delete[](a->x1); + delete[](a->x0); + delete(a); +} + +void BQLP::flush_bqlp(BQLP *a) +{ + int i; + for (i = 0; i < a->nstages; i++) + { + a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; + a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; + } +} + +void BQLP::xbqlp(BQLP *a) +{ + if (a->run) + { + int i, j, n; + + for (i = 0; i < a->size; i++) + { + for (j = 0; j < 2; j++) + { + a->x0[j] = a->gain * a->in[2 * i + j]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; + a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] + + a->a1 * a->x1[2 * n + j] + + a->a2 * a->x2[2 * n + j] + + a->b1 * a->y1[2 * n + j] + + a->b2 * a->y2[2 * n + j]; + a->y2[2 * n + j] = a->y1[2 * n + j]; + a->y1[2 * n + j] = a->y0[2 * n + j]; + a->x2[2 * n + j] = a->x1[2 * n + j]; + a->x1[2 * n + j] = a->x0[2 * n + j]; + } + + a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; + } + } + } + else if (a->out != a->in) + { + std::copy(a->in, a->in + a->size * 2, a->out); + } +} + +void BQLP::setBuffers_bqlp(BQLP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void BQLP::setSamplerate_bqlp(BQLP *a, int rate) +{ + a->rate = rate; + calc_bqlp(a); +} + +void BQLP::setSize_bqlp(BQLP *a, int size) +{ + a->size = size; + flush_bqlp(a); +} + + +} // namespace WDSP diff --git a/wdsp/bqlp.hpp b/wdsp/bqlp.hpp new file mode 100644 index 000000000..49672c765 --- /dev/null +++ b/wdsp/bqlp.hpp @@ -0,0 +1,70 @@ +/* bqlp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Complex Bi-Quad Low-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_bqlp_h +#define wdsp_bqlp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API BQLP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double fc; + double Q; + double gain; + int nstages; + double a0, a1, a2, b1, b2; + double* x0, * x1, * x2, * y0, * y1, * y2; + + static BQLP* create_bqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages); + static void destroy_bqlp(BQLP *a); + static void flush_bqlp(BQLP *a); + static void xbqlp(BQLP *a); + static void setBuffers_bqlp(BQLP *a, float* in, float* out); + static void setSamplerate_bqlp(BQLP *a, int rate); + static void setSize_bqlp(BQLP *a, int size); + +private: + static void calc_bqlp(BQLP *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/dbqbp.cpp b/wdsp/dbqbp.cpp new file mode 100644 index 000000000..0404a80f6 --- /dev/null +++ b/wdsp/dbqbp.cpp @@ -0,0 +1,155 @@ +/* dbqbp.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "dbqbp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Double Bi-Quad Band-Pass * +* * +********************************************************************************************************/ + +void DBQBP::calc_dbqbp(DBQBP *a) +{ + double f0, w0, bw, q, sn, cs, c, den; + bw = a->f_high - a->f_low; + f0 = (a->f_high + a->f_low) / 2.0; + q = f0 / bw; + w0 = TWOPI * f0 / a->rate; + sn = sin(w0); + cs = cos(w0); + c = sn / (2.0 * q); + den = 1.0 + c; + a->a0 = +c / den; + a->a1 = 0.0; + a->a2 = -c / den; + a->b1 = 2.0 * cs / den; + a->b2 = (c - 1.0) / den; + flush_dbqbp(a); +} + +DBQBP* DBQBP::create_dbqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages) +{ + DBQBP *a = new DBQBP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->f_low = f_low; + a->f_high = f_high; + a->gain = gain; + a->nstages = nstages; + a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->x2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + calc_dbqbp(a); + return a; +} + +void DBQBP::destroy_dbqbp(DBQBP *a) +{ + delete[](a->y2); + delete[](a->y1); + delete[](a->y0); + delete[](a->x2); + delete[](a->x1); + delete[](a->x0); + delete(a); +} + +void DBQBP::flush_dbqbp(DBQBP *a) +{ + int i; + for (i = 0; i < a->nstages; i++) + { + a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0; + } +} + +void DBQBP::xdbqbp(DBQBP *a) +{ + if (a->run) + { + int i, n; + + for (i = 0; i < a->size; i++) + { + a->x0[0] = a->gain * a->in[i]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[n] = a->y0[n - 1]; + + a->y0[n] = a->a0 * a->x0[n] + + a->a1 * a->x1[n] + + a->a2 * a->x2[n] + + a->b1 * a->y1[n] + + a->b2 * a->y2[n]; + a->y2[n] = a->y1[n]; + a->y1[n] = a->y0[n]; + a->x2[n] = a->x1[n]; + a->x1[n] = a->x0[n]; + } + + a->out[i] = a->y0[a->nstages - 1]; + } + } + else if (a->out != a->in) + { + memcpy(a->out, a->in, a->size * sizeof(float)); + } +} + +void DBQBP::setBuffers_dbqbp(DBQBP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void DBQBP::setSamplerate_dbqbp(DBQBP *a, int rate) +{ + a->rate = rate; + calc_dbqbp(a); +} + +void DBQBP::setSize_dbqbp(DBQBP *a, int size) +{ + a->size = size; + flush_dbqbp(a); +} + +} // namespace WDSP diff --git a/wdsp/dbqbp.hpp b/wdsp/dbqbp.hpp new file mode 100644 index 000000000..36051020e --- /dev/null +++ b/wdsp/dbqbp.hpp @@ -0,0 +1,71 @@ +/* dbqbp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Complex Bi-Quad Band-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_dbqbp_h +#define wdsp_dbqbp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API DBQBP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double f_low; + double f_high; + double gain; + int nstages; + double a0, a1, a2, b1, b2; + double* x0, * x1, * x2, * y0, * y1, * y2; + + // Double Bi-Quad Band-Pass + static DBQBP* create_dbqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages); + static void destroy_dbqbp(DBQBP *a); + static void flush_dbqbp(DBQBP *a); + static void xdbqbp(DBQBP *a); + static void setBuffers_dbqbp(DBQBP *a, float* in, float* out); + static void setSamplerate_dbqbp(DBQBP *a, int rate); + static void setSize_dbqbp(DBQBP *a, int size); + +private: + static void calc_dbqbp(DBQBP *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/dbqlp.cpp b/wdsp/dbqlp.cpp new file mode 100644 index 000000000..3062b8ab3 --- /dev/null +++ b/wdsp/dbqlp.cpp @@ -0,0 +1,151 @@ +/* dbqlp.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "dbqlp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Double Bi-Quad Low-Pass * +* * +********************************************************************************************************/ + +void DBQLP::calc_dbqlp(DBQLP *a) +{ + float w0, cs, c, den; + w0 = TWOPI * a->fc / (float)a->rate; + cs = cos(w0); + c = sin(w0) / (2.0 * a->Q); + den = 1.0 + c; + a->a0 = 0.5 * (1.0 - cs) / den; + a->a1 = (1.0 - cs) / den; + a->a2 = 0.5 * (1.0 - cs) / den; + a->b1 = 2.0 * cs / den; + a->b2 = (c - 1.0) / den; + flush_dbqlp(a); +} + +DBQLP* DBQLP::create_dbqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages) +{ + DBQLP *a = new DBQLP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->fc = fc; + a->Q = Q; + a->gain = gain; + a->nstages = nstages; + a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->x2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + calc_dbqlp(a); + return a; +} + +void DBQLP::destroy_dbqlp(DBQLP *a) +{ + delete[](a->y2); + delete[](a->y1); + delete[](a->y0); + delete[](a->x2); + delete[](a->x1); + delete[](a->x0); + delete(a); +} + +void DBQLP::flush_dbqlp(DBQLP *a) +{ + int i; + for (i = 0; i < a->nstages; i++) + { + a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0; + } +} + +void DBQLP::xdbqlp(DBQLP *a) +{ + if (a->run) + { + int i, n; + + for (i = 0; i < a->size; i++) + { + a->x0[0] = a->gain * a->in[i]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[n] = a->y0[n - 1]; + + a->y0[n] = a->a0 * a->x0[n] + + a->a1 * a->x1[n] + + a->a2 * a->x2[n] + + a->b1 * a->y1[n] + + a->b2 * a->y2[n]; + a->y2[n] = a->y1[n]; + a->y1[n] = a->y0[n]; + a->x2[n] = a->x1[n]; + a->x1[n] = a->x0[n]; + } + + a->out[i] = a->y0[a->nstages - 1]; + } + } + else if (a->out != a->in) + { + memcpy(a->out, a->in, a->size * sizeof(float)); + } +} + +void DBQLP::setBuffers_dbqlp(DBQLP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void DBQLP::setSamplerate_dbqlp(DBQLP *a, int rate) +{ + a->rate = rate; + calc_dbqlp(a); +} + +void DBQLP::setSize_dbqlp(DBQLP *a, int size) +{ + a->size = size; + flush_dbqlp(a); +} + +} // namespace WDSP diff --git a/wdsp/dbqlp.hpp b/wdsp/dbqlp.hpp new file mode 100644 index 000000000..c6673944e --- /dev/null +++ b/wdsp/dbqlp.hpp @@ -0,0 +1,70 @@ +/* dbqlp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Double Bi-Quad Low-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_dbqlp_h +#define wdsp_dbqlp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API DBQLP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double fc; + double Q; + double gain; + int nstages; + double a0, a1, a2, b1, b2; + double* x0, * x1, * x2, * y0, * y1, * y2; + + static DBQLP* create_dbqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages); + static void destroy_dbqlp(DBQLP *a); + static void flush_dbqlp(DBQLP *a); + static void xdbqlp(DBQLP *a); + static void setBuffers_dbqlp(DBQLP *a, float* in, float* out); + static void setSamplerate_dbqlp(DBQLP *a, int rate); + static void setSize_dbqlp(DBQLP *a, int size); + +private: + static void calc_dbqlp(DBQLP *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/dsphp.cpp b/wdsp/dsphp.cpp new file mode 100644 index 000000000..c6ae2d527 --- /dev/null +++ b/wdsp/dsphp.cpp @@ -0,0 +1,140 @@ +/* iir.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "dsphp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Double Single-Pole High-Pass * +* * +********************************************************************************************************/ + +void DSPHP::calc_dsphp(DSPHP *a) +{ + double g; + a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); + g = exp(-TWOPI * a->fc / a->rate); + a->b0 = +0.5 * (1.0 + g); + a->b1 = -0.5 * (1.0 + g); + a->a1 = -g; +} + +DSPHP* DSPHP::create_dsphp(int run, int size, float* in, float* out, double rate, double fc, int nstages) +{ + DSPHP *a = new DSPHP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->fc = fc; + a->nstages = nstages; + calc_dsphp(a); + return a; +} + +void DSPHP::decalc_dsphp(DSPHP *a) +{ + delete[](a->y1); + delete[](a->y0); + delete[](a->x1); + delete[](a->x0); +} + +void DSPHP::destroy_dsphp(DSPHP *a) +{ + decalc_dsphp(a); + delete(a); +} + +void DSPHP::flush_dsphp(DSPHP *a) +{ + memset(a->x0, 0, a->nstages * sizeof(float)); + memset(a->x1, 0, a->nstages * sizeof(float)); + memset(a->y0, 0, a->nstages * sizeof(float)); + memset(a->y1, 0, a->nstages * sizeof(float)); +} + +void DSPHP::xdsphp(DSPHP *a) +{ + if (a->run) + { + int i, n; + + for (i = 0; i < a->size; i++) + { + a->x0[0] = a->in[i]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[n] = a->y0[n - 1]; + + a->y0[n] = a->b0 * a->x0[n] + + a->b1 * a->x1[n] + - a->a1 * a->y1[n]; + a->y1[n] = a->y0[n]; + a->x1[n] = a->x0[n]; + } + + a->out[i] = a->y0[a->nstages - 1]; + } + } + else if (a->out != a->in) + { + memcpy(a->out, a->in, a->size * sizeof(float)); + } +} + +void DSPHP::setBuffers_dsphp(DSPHP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void DSPHP::setSamplerate_dsphp(DSPHP *a, int rate) +{ + decalc_dsphp(a); + a->rate = rate; + calc_dsphp(a); +} + +void DSPHP::setSize_dsphp(DSPHP *a, int size) +{ + a->size = size; + flush_dsphp(a); +} + +} // namespace WDSP diff --git a/wdsp/dsphp.hpp b/wdsp/dsphp.hpp new file mode 100644 index 000000000..4b5b93b19 --- /dev/null +++ b/wdsp/dsphp.hpp @@ -0,0 +1,71 @@ +/* sphp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Double Single-Pole High-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_dsphp_h +#define wdsp_dsphp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API DSPHP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double fc; + int nstages; + double a1, b0, b1; + double* x0, * x1, * y0, * y1; + + static DSPHP* create_dsphp(int run, int size, float* in, float* out, double rate, double fc, int nstages); + static void destroy_dsphp(DSPHP *a); + static void flush_dsphp(DSPHP *a); + static void xdsphp(DSPHP *a); + static void setBuffers_dsphp(DSPHP *a, float* in, float* out); + static void setSamplerate_dsphp(DSPHP *a, int rate); + static void setSize_dsphp(DSPHP *a, int size); + +private: + static void calc_dsphp(DSPHP *a); + static void decalc_dsphp(DSPHP *a); +}; + +} // namespace WDSP + +#endif + + diff --git a/wdsp/fmd.cpp b/wdsp/fmd.cpp index bac132be5..e762dfcce 100644 --- a/wdsp/fmd.cpp +++ b/wdsp/fmd.cpp @@ -26,11 +26,11 @@ warren@wpratt.com */ #include "comm.hpp" -#include "iir.hpp" #include "fircore.hpp" #include "fcurve.hpp" #include "fir.hpp" #include "wcpAGC.hpp" +#include "snotch.hpp" #include "fmd.hpp" namespace WDSP { diff --git a/wdsp/iir.cpp b/wdsp/iir.cpp deleted file mode 100644 index 7080d5744..000000000 --- a/wdsp/iir.cpp +++ /dev/null @@ -1,1387 +0,0 @@ -/* iir.c - -This file is part of a program that implements a Software-Defined Radio. - -Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V -Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel - -This program is free software; you can redistribute it and/or -modify it under the terms of the GNU General Public License -as published by the Free Software Foundation; either version 2 -of the License, or (at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. - -The author can be reached by email at - -warren@wpratt.com - -*/ - -#include "comm.hpp" -#include "iir.hpp" -#include "RXA.hpp" -#include "TXA.hpp" - -namespace WDSP { - -/******************************************************************************************************** -* * -* Bi-Quad Notch * -* * -********************************************************************************************************/ - -void SNOTCH::calc_snotch (SNOTCH *a) -{ - double fn, qk, qr, csn; - fn = a->f / (float)a->rate; - csn = cos (TWOPI * fn); - qr = 1.0 - 3.0 * a->bw; - qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn)); - a->a0 = + qk; - a->a1 = - 2.0 * qk * csn; - a->a2 = + qk; - a->b1 = + 2.0 * qr * csn; - a->b2 = - qr * qr; - flush_snotch (a); -} - -SNOTCH* SNOTCH::create_snotch (int run, int size, float* in, float* out, int rate, double f, double bw) -{ - SNOTCH *a = new SNOTCH; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->f = f; - a->bw = bw; - calc_snotch (a); - return a; -} - -void SNOTCH::destroy_snotch (SNOTCH *a) -{ - delete (a); -} - -void SNOTCH::flush_snotch (SNOTCH *a) -{ - a->x1 = a->x2 = a->y1 = a->y2 = 0.0; -} - -void SNOTCH::xsnotch (SNOTCH *a) -{ - if (a->run) - { - int i; - for (i = 0; i < a->size; i++) - { - a->x0 = a->in[2 * i + 0]; - a->out[2 * i + 0] = a->a0 * a->x0 + a->a1 * a->x1 + a->a2 * a->x2 + a->b1 * a->y1 + a->b2 * a->y2; - a->y2 = a->y1; - a->y1 = a->out[2 * i + 0]; - a->x2 = a->x1; - a->x1 = a->x0; - } - } - else if (a->out != a->in) - { - std::copy( a->in, a->in + a->size * 2, a->out); - } -} - -void SNOTCH::setBuffers_snotch (SNOTCH *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void SNOTCH::setSamplerate_snotch (SNOTCH *a, int rate) -{ - a->rate = rate; - calc_snotch (a); -} - -void SNOTCH::setSize_snotch (SNOTCH *a, int size) -{ - a->size = size; - flush_snotch (a); -} - -/******************************************************************************************************** -* * -* RXA Properties * -* * -********************************************************************************************************/ - -void SNOTCH::SetSNCTCSSFreq (SNOTCH *a, double freq) -{ - a->f = freq; - calc_snotch (a); -} - -void SNOTCH::SetSNCTCSSRun (SNOTCH *a, int run) -{ - a->run = run; -} - - -/******************************************************************************************************** -* * -* Complex Bi-Quad Peaking * -* * -********************************************************************************************************/ - -void SPEAK::calc_speak (SPEAK *a) -{ - double ratio; - double f_corr, g_corr, bw_corr, bw_parm, A, f_min; - - switch (a->design) - { - case 0: - ratio = a->bw / a->f; - switch (a->nstages) - { - case 4: - bw_parm = 2.4; - f_corr = 1.0 - 0.160 * ratio + 1.440 * ratio * ratio; - g_corr = 1.0 - 1.003 * ratio + 3.990 * ratio * ratio; - break; - default: - bw_parm = 1.0; - f_corr = 1.0; - g_corr = 1.0; - break; - } - { - double fn, qk, qr, csn; - a->fgain = a->gain / g_corr; - fn = a->f / (double)a->rate / f_corr; - csn = cos (TWOPI * fn); - qr = 1.0 - 3.0 * a->bw / (double)a->rate * bw_parm; - qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn)); - a->a0 = 1.0 - qk; - a->a1 = 2.0 * (qk - qr) * csn; - a->a2 = qr * qr - qk; - a->b1 = 2.0 * qr * csn; - a->b2 = - qr * qr; - } - break; - - case 1: - if (a->f < 200.0) a->f = 200.0; - ratio = a->bw / a->f; - switch (a->nstages) - { - case 4: - bw_parm = 5.0; - bw_corr = 1.13 * ratio - 0.956 * ratio * ratio; - A = 2.5; - f_min = 50.0; - break; - default: - bw_parm = 1.0; - bw_corr = 1.0; - g_corr = 1.0; - A = 2.5; - f_min = 50.0; - break; - } - { - double w0, sn, c, den; - if (a->f < f_min) a->f = f_min; - w0 = TWOPI * a->f / (double)a->rate; - sn = sin (w0); - a->cbw = bw_corr * a->f; - c = sn * sinh(0.5 * log((a->f + 0.5 * a->cbw * bw_parm) / (a->f - 0.5 * a->cbw * bw_parm)) * w0 / sn); - den = 1.0 + c / A; - a->a0 = (1.0 + c * A) / den; - a->a1 = - 2.0 * cos (w0) / den; - a->a2 = (1 - c * A) / den; - a->b1 = - a->a1; - a->b2 = - (1 - c / A ) / den; - a->fgain = a->gain / pow (A * A, (double)a->nstages); - } - break; - } - flush_speak (a); -} - -SPEAK* SPEAK::create_speak ( - int run, - int size, - float* in, - float* out, - int rate, - double f, - double bw, - double gain, - int nstages, - int design -) -{ - SPEAK *a = new SPEAK; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->f = f; - a->bw = bw; - a->gain = gain; - a->nstages = nstages; - a->design = design; - a->x0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); - a->x1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); - a->x2 = new double[a->nstages * 2]; //(float *) malloc0 (a->nstages * sizeof (complex)); - a->y0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); - a->y1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); - a->y2 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); - calc_speak (a); - return a; -} - -void SPEAK::destroy_speak (SPEAK *a) -{ - delete[] (a->y2); - delete[] (a->y1); - delete[] (a->y0); - delete[] (a->x2); - delete[] (a->x1); - delete[] (a->x0); - delete (a); -} - -void SPEAK::flush_speak (SPEAK *a) -{ - int i; - for (i = 0; i < a->nstages; i++) - { - a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; - a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; - } -} - -void SPEAK::xspeak (SPEAK *a) -{ - if (a->run) - { - int i, j, n; - - for (i = 0; i < a->size; i++) - { - for (j = 0; j < 2; j++) - { - a->x0[j] = a->fgain * a->in[2 * i + j]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; - a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] - + a->a1 * a->x1[2 * n + j] - + a->a2 * a->x2[2 * n + j] - + a->b1 * a->y1[2 * n + j] - + a->b2 * a->y2[2 * n + j]; - a->y2[2 * n + j] = a->y1[2 * n + j]; - a->y1[2 * n + j] = a->y0[2 * n + j]; - a->x2[2 * n + j] = a->x1[2 * n + j]; - a->x1[2 * n + j] = a->x0[2 * n + j]; - } - - a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; - } - } - } - else if (a->out != a->in) - { - std::copy( a->in, a->in + a->size * 2, a->out); - } -} - -void SPEAK::setBuffers_speak (SPEAK *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void SPEAK::setSamplerate_speak (SPEAK *a, int rate) -{ - a->rate = rate; - calc_speak (a); -} - -void SPEAK::setSize_speak (SPEAK *a, int size) -{ - a->size = size; - flush_speak (a); -} - -/******************************************************************************************************** -* * -* RXA Properties * -* * -********************************************************************************************************/ - -void SPEAK::SetSPCWRun (RXA& rxa, int run) -{ - SPEAK *a = rxa.speak; - a->run = run; -} - -void SPEAK::SetSPCWFreq (RXA& rxa, double freq) -{ - SPEAK *a = rxa.speak; - a->f = freq; - calc_speak (a); -} - -void SPEAK::SetSPCWBandwidth (RXA& rxa, double bw) -{ - SPEAK *a = rxa.speak; - a->bw = bw; - calc_speak (a); -} - -void SPEAK::SetSPCWGain (RXA& rxa, double gain) -{ - SPEAK *a = rxa.speak; - a->gain = gain; - calc_speak (a); -} - -/******************************************************************************************************** -* * -* Complex Multiple Peaking * -* * -********************************************************************************************************/ - -void MPEAK::calc_mpeak (MPEAK *a) -{ - int i; - a->tmp = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex)); - a->mix = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex)); - for (i = 0; i < a->npeaks; i++) - { - a->pfil[i] = SPEAK::create_speak ( - 1, - a->size, - a->in, - a->tmp, - a->rate, - a->f[i], - a->bw[i], - a->gain[i], - a->nstages, - 1 - ); - } -} - -void MPEAK::decalc_mpeak (MPEAK *a) -{ - int i; - for (i = 0; i < a->npeaks; i++) - SPEAK::destroy_speak (a->pfil[i]); - delete[] (a->mix); - delete[] (a->tmp); -} - -MPEAK* MPEAK::create_mpeak ( - int run, - int size, - float* in, - float* out, - int rate, - int npeaks, - int* enable, - double* f, - double* bw, - double* gain, - int nstages -) -{ - MPEAK *a = new MPEAK; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->npeaks = npeaks; - a->nstages = nstages; - a->enable = new int[a->npeaks]; // (int *) malloc0 (a->npeaks * sizeof (int)); - a->f = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); - a->bw = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); - a->gain = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); - memcpy (a->enable, enable, a->npeaks * sizeof (int)); - memcpy (a->f, f, a->npeaks * sizeof (double)); - memcpy (a->bw, bw, a->npeaks * sizeof (double)); - memcpy (a->gain, gain, a->npeaks * sizeof (double)); - a->pfil = new SPEAK*[a->npeaks]; // (SPEAK *) malloc0 (a->npeaks * sizeof (SPEAK)); - calc_mpeak (a); - return a; -} - -void MPEAK::destroy_mpeak (MPEAK *a) -{ - decalc_mpeak (a); - delete[] (a->pfil); - delete[] (a->gain); - delete[] (a->bw); - delete[] (a->f); - delete[] (a->enable); - delete (a); -} - -void MPEAK::flush_mpeak (MPEAK *a) -{ - int i; - for (i = 0; i < a->npeaks; i++) - SPEAK::flush_speak (a->pfil[i]); -} - -void MPEAK::xmpeak (MPEAK *a) -{ - if (a->run) - { - int i, j; - std::fill(a->mix, a->mix + a->size * 2, 0); - - for (i = 0; i < a->npeaks; i++) - { - if (a->enable[i]) - { - SPEAK::xspeak (a->pfil[i]); - for (j = 0; j < 2 * a->size; j++) - a->mix[j] += a->tmp[j]; - } - } - - std::copy(a->mix, a->mix + a->size * 2, a->out); - } - else if (a->in != a->out) - { - std::copy( a->in, a->in + a->size * 2, a->out); - } -} - -void MPEAK::setBuffers_mpeak (MPEAK *a, float* in, float* out) -{ - decalc_mpeak (a); - a->in = in; - a->out = out; - calc_mpeak (a); -} - -void MPEAK::setSamplerate_mpeak (MPEAK *a, int rate) -{ - decalc_mpeak (a); - a->rate = rate; - calc_mpeak (a); -} - -void MPEAK::setSize_mpeak (MPEAK *a, int size) -{ - decalc_mpeak (a); - a->size = size; - calc_mpeak (a); -} - -/******************************************************************************************************** -* * -* RXA Properties * -* * -********************************************************************************************************/ - -void MPEAK::SetmpeakRun (RXA& rxa, int run) -{ - MPEAK *a = rxa.mpeak; - a->run = run; -} - -void MPEAK::SetmpeakNpeaks (RXA& rxa, int npeaks) -{ - MPEAK *a = rxa.mpeak; - a->npeaks = npeaks; -} - -void MPEAK::SetmpeakFilEnable (RXA& rxa, int fil, int enable) -{ - MPEAK *a = rxa.mpeak; - a->enable[fil] = enable; -} - -void MPEAK::SetmpeakFilFreq (RXA& rxa, int fil, double freq) -{ - MPEAK *a = rxa.mpeak; - a->f[fil] = freq; - a->pfil[fil]->f = freq; - SPEAK::calc_speak(a->pfil[fil]); -} - -void MPEAK::SetmpeakFilBw (RXA& rxa, int fil, double bw) -{ - MPEAK *a = rxa.mpeak; - a->bw[fil] = bw; - a->pfil[fil]->bw = bw; - SPEAK::calc_speak(a->pfil[fil]); -} - -void MPEAK::SetmpeakFilGain (RXA& rxa, int fil, double gain) -{ - MPEAK *a = rxa.mpeak; - a->gain[fil] = gain; - a->pfil[fil]->gain = gain; - SPEAK::calc_speak(a->pfil[fil]); -} - - -/******************************************************************************************************** -* * -* Phase Rotator * -* * -********************************************************************************************************/ - -void PHROT::calc_phrot (PHROT *a) -{ - double g; - a->x0 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); - a->x1 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); - a->y0 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); - a->y1 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); - g = tan (PI * a->fc / (float)a->rate); - a->b0 = (g - 1.0) / (g + 1.0); - a->b1 = 1.0; - a->a1 = a->b0; -} - -void PHROT::decalc_phrot (PHROT *a) -{ - delete[] (a->y1); - delete[] (a->y0); - delete[] (a->x1); - delete[] (a->x0); -} - -PHROT* PHROT::create_phrot (int run, int size, float* in, float* out, int rate, double fc, int nstages) -{ - PHROT *a = new PHROT; - a->reverse = 0; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->fc = fc; - a->nstages = nstages; - calc_phrot (a); - return a; -} - -void PHROT::destroy_phrot (PHROT *a) -{ - decalc_phrot (a); - delete (a); -} - -void PHROT::flush_phrot (PHROT *a) -{ - memset (a->x0, 0, a->nstages * sizeof (double)); - memset (a->x1, 0, a->nstages * sizeof (double)); - memset (a->y0, 0, a->nstages * sizeof (double)); - memset (a->y1, 0, a->nstages * sizeof (double)); -} - -void PHROT::xphrot (PHROT *a) -{ - if (a->reverse) - { - for (int i = 0; i < a->size; i++) - a->in[2 * i + 0] = -a->in[2 * i + 0]; - } - - if (a->run) - { - int i, n; - - for (i = 0; i < a->size; i++) - { - a->x0[0] = a->in[2 * i + 0]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) a->x0[n] = a->y0[n - 1]; - a->y0[n] = a->b0 * a->x0[n] - + a->b1 * a->x1[n] - - a->a1 * a->y1[n]; - a->y1[n] = a->y0[n]; - a->x1[n] = a->x0[n]; - } - - a->out[2 * i + 0] = a->y0[a->nstages - 1]; - } - } - else if (a->out != a->in) - { - std::copy( a->in, a->in + a->size * 2, a->out); - } -} - -void PHROT::setBuffers_phrot (PHROT *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void PHROT::setSamplerate_phrot (PHROT *a, int rate) -{ - decalc_phrot (a); - a->rate = rate; - calc_phrot (a); -} - -void PHROT::setSize_phrot (PHROT *a, int size) -{ - a->size = size; - flush_phrot (a); -} - -/******************************************************************************************************** -* * -* TXA Properties * -* * -********************************************************************************************************/ - -void PHROT::SetPHROTRun (TXA& txa, int run) -{ - PHROT *a = txa.phrot; - a->run = run; - - if (a->run) - flush_phrot (a); -} - -void PHROT::SetPHROTCorner (TXA& txa, double corner) -{ - PHROT *a = txa.phrot; - decalc_phrot (a); - a->fc = corner; - calc_phrot (a); -} - -void PHROT::SetPHROTNstages (TXA& txa, int nstages) -{ - PHROT *a = txa.phrot; - decalc_phrot (a); - a->nstages = nstages; - calc_phrot (a); -} - -void PHROT::SetPHROTReverse (TXA& txa, int reverse) -{ - PHROT *a = txa.phrot; - a->reverse = reverse; -} - -/******************************************************************************************************** -* * -* Complex Bi-Quad Low-Pass * -* * -********************************************************************************************************/ - -void BQLP::calc_bqlp(BQLP *a) -{ - double w0, cs, c, den; - w0 = TWOPI * a->fc / (double)a->rate; - cs = cos(w0); - c = sin(w0) / (2.0 * a->Q); - den = 1.0 + c; - a->a0 = 0.5 * (1.0 - cs) / den; - a->a1 = (1.0 - cs) / den; - a->a2 = 0.5 * (1.0 - cs) / den; - a->b1 = 2.0 * cs / den; - a->b2 = (c - 1.0) / den; - flush_bqlp(a); -} - -BQLP* BQLP::create_bqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages) -{ - BQLP *a = new BQLP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->fc = fc; - a->Q = Q; - a->gain = gain; - a->nstages = nstages; - a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->x2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - calc_bqlp(a); - return a; -} - -void BQLP::destroy_bqlp(BQLP *a) -{ - delete[](a->y2); - delete[](a->y1); - delete[](a->y0); - delete[](a->x2); - delete[](a->x1); - delete[](a->x0); - delete(a); -} - -void BQLP::flush_bqlp(BQLP *a) -{ - int i; - for (i = 0; i < a->nstages; i++) - { - a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; - a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; - } -} - -void BQLP::xbqlp(BQLP *a) -{ - if (a->run) - { - int i, j, n; - - for (i = 0; i < a->size; i++) - { - for (j = 0; j < 2; j++) - { - a->x0[j] = a->gain * a->in[2 * i + j]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; - a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] - + a->a1 * a->x1[2 * n + j] - + a->a2 * a->x2[2 * n + j] - + a->b1 * a->y1[2 * n + j] - + a->b2 * a->y2[2 * n + j]; - a->y2[2 * n + j] = a->y1[2 * n + j]; - a->y1[2 * n + j] = a->y0[2 * n + j]; - a->x2[2 * n + j] = a->x1[2 * n + j]; - a->x1[2 * n + j] = a->x0[2 * n + j]; - } - - a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; - } - } - } - else if (a->out != a->in) - { - std::copy(a->in, a->in + a->size * 2, a->out); - } -} - -void BQLP::setBuffers_bqlp(BQLP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void BQLP::setSamplerate_bqlp(BQLP *a, int rate) -{ - a->rate = rate; - calc_bqlp(a); -} - -void BQLP::setSize_bqlp(BQLP *a, int size) -{ - a->size = size; - flush_bqlp(a); -} - -/******************************************************************************************************** -* * -* Double Bi-Quad Low-Pass * -* * -********************************************************************************************************/ - -void DBQLP::calc_dbqlp(BQLP *a) -{ - float w0, cs, c, den; - w0 = TWOPI * a->fc / (float)a->rate; - cs = cos(w0); - c = sin(w0) / (2.0 * a->Q); - den = 1.0 + c; - a->a0 = 0.5 * (1.0 - cs) / den; - a->a1 = (1.0 - cs) / den; - a->a2 = 0.5 * (1.0 - cs) / den; - a->b1 = 2.0 * cs / den; - a->b2 = (c - 1.0) / den; - flush_dbqlp(a); -} - -BQLP* DBQLP::create_dbqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages) -{ - BQLP *a = new BQLP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->fc = fc; - a->Q = Q; - a->gain = gain; - a->nstages = nstages; - a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->x2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - calc_dbqlp(a); - return a; -} - -void DBQLP::destroy_dbqlp(BQLP *a) -{ - delete[](a->y2); - delete[](a->y1); - delete[](a->y0); - delete[](a->x2); - delete[](a->x1); - delete[](a->x0); - delete(a); -} - -void DBQLP::flush_dbqlp(BQLP *a) -{ - int i; - for (i = 0; i < a->nstages; i++) - { - a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0; - } -} - -void DBQLP::xdbqlp(BQLP *a) -{ - if (a->run) - { - int i, n; - - for (i = 0; i < a->size; i++) - { - a->x0[0] = a->gain * a->in[i]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[n] = a->y0[n - 1]; - - a->y0[n] = a->a0 * a->x0[n] - + a->a1 * a->x1[n] - + a->a2 * a->x2[n] - + a->b1 * a->y1[n] - + a->b2 * a->y2[n]; - a->y2[n] = a->y1[n]; - a->y1[n] = a->y0[n]; - a->x2[n] = a->x1[n]; - a->x1[n] = a->x0[n]; - } - - a->out[i] = a->y0[a->nstages - 1]; - } - } - else if (a->out != a->in) - { - memcpy(a->out, a->in, a->size * sizeof(float)); - } -} - -void DBQLP::setBuffers_dbqlp(BQLP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void DBQLP::setSamplerate_dbqlp(BQLP *a, int rate) -{ - a->rate = rate; - calc_dbqlp(a); -} - -void DBQLP::setSize_dbqlp(BQLP *a, int size) -{ - a->size = size; - flush_dbqlp(a); -} - - -/******************************************************************************************************** -* * -* Complex Bi-Quad Band-Pass * -* * -********************************************************************************************************/ - -void BQBP::calc_bqbp(BQBP *a) -{ - double f0, w0, bw, q, sn, cs, c, den; - bw = a->f_high - a->f_low; - f0 = (a->f_high + a->f_low) / 2.0; - q = f0 / bw; - w0 = TWOPI * f0 / a->rate; - sn = sin(w0); - cs = cos(w0); - c = sn / (2.0 * q); - den = 1.0 + c; - a->a0 = +c / den; - a->a1 = 0.0; - a->a2 = -c / den; - a->b1 = 2.0 * cs / den; - a->b2 = (c - 1.0) / den; - flush_bqbp(a); -} - -BQBP* BQBP::create_bqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages) -{ - BQBP *a = new BQBP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->f_low = f_low; - a->f_high = f_high; - a->gain = gain; - a->nstages = nstages; - a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->x2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y2 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - calc_bqbp(a); - return a; -} - -void BQBP::destroy_bqbp(BQBP *a) -{ - delete[](a->y2); - delete[](a->y1); - delete[](a->y0); - delete[](a->x2); - delete[](a->x1); - delete[](a->x0); - delete(a); -} - -void BQBP::flush_bqbp(BQBP *a) -{ - int i; - for (i = 0; i < a->nstages; i++) - { - a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; - a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; - } -} - -void BQBP::xbqbp(BQBP *a) -{ - if (a->run) - { - int i, j, n; - - for (i = 0; i < a->size; i++) - { - for (j = 0; j < 2; j++) - { - a->x0[j] = a->gain * a->in[2 * i + j]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; - - a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] - + a->a1 * a->x1[2 * n + j] - + a->a2 * a->x2[2 * n + j] - + a->b1 * a->y1[2 * n + j] - + a->b2 * a->y2[2 * n + j]; - a->y2[2 * n + j] = a->y1[2 * n + j]; - a->y1[2 * n + j] = a->y0[2 * n + j]; - a->x2[2 * n + j] = a->x1[2 * n + j]; - a->x1[2 * n + j] = a->x0[2 * n + j]; - } - - a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; - } - } - } - else if (a->out != a->in) - { - std::copy(a->in, a->in + a->size * 2, a->out); - } -} - -void BQBP::setBuffers_bqbp(BQBP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void BQBP::setSamplerate_bqbp(BQBP *a, int rate) -{ - a->rate = rate; - calc_bqbp(a); -} - -void BQBP::setSize_bqbp(BQBP *a, int size) -{ - a->size = size; - flush_bqbp(a); -} - -/******************************************************************************************************** -* * -* Double Bi-Quad Band-Pass * -* * -********************************************************************************************************/ - -void BQBP::calc_dbqbp(BQBP *a) -{ - double f0, w0, bw, q, sn, cs, c, den; - bw = a->f_high - a->f_low; - f0 = (a->f_high + a->f_low) / 2.0; - q = f0 / bw; - w0 = TWOPI * f0 / a->rate; - sn = sin(w0); - cs = cos(w0); - c = sn / (2.0 * q); - den = 1.0 + c; - a->a0 = +c / den; - a->a1 = 0.0; - a->a2 = -c / den; - a->b1 = 2.0 * cs / den; - a->b2 = (c - 1.0) / den; - flush_dbqbp(a); -} - -BQBP* BQBP::create_dbqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages) -{ - BQBP *a = new BQBP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->f_low = f_low; - a->f_high = f_high; - a->gain = gain; - a->nstages = nstages; - a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->x2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y2 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - calc_dbqbp(a); - return a; -} - -void BQBP::destroy_dbqbp(BQBP *a) -{ - delete[](a->y2); - delete[](a->y1); - delete[](a->y0); - delete[](a->x2); - delete[](a->x1); - delete[](a->x0); - delete(a); -} - -void BQBP::flush_dbqbp(BQBP *a) -{ - int i; - for (i = 0; i < a->nstages; i++) - { - a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0; - } -} - -void BQBP::xdbqbp(BQBP *a) -{ - if (a->run) - { - int i, n; - - for (i = 0; i < a->size; i++) - { - a->x0[0] = a->gain * a->in[i]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[n] = a->y0[n - 1]; - - a->y0[n] = a->a0 * a->x0[n] - + a->a1 * a->x1[n] - + a->a2 * a->x2[n] - + a->b1 * a->y1[n] - + a->b2 * a->y2[n]; - a->y2[n] = a->y1[n]; - a->y1[n] = a->y0[n]; - a->x2[n] = a->x1[n]; - a->x1[n] = a->x0[n]; - } - - a->out[i] = a->y0[a->nstages - 1]; - } - } - else if (a->out != a->in) - { - memcpy(a->out, a->in, a->size * sizeof(float)); - } -} - -void BQBP::setBuffers_dbqbp(BQBP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void BQBP::setSamplerate_dbqbp(BQBP *a, int rate) -{ - a->rate = rate; - calc_dbqbp(a); -} - -void BQBP::setSize_dbqbp(BQBP *a, int size) -{ - a->size = size; - flush_dbqbp(a); -} - -/******************************************************************************************************** -* * -* Complex Single-Pole High-Pass * -* * -********************************************************************************************************/ - -void SPHP::calc_sphp(SPHP *a) -{ - double g; - a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); - g = exp(-TWOPI * a->fc / a->rate); - a->b0 = +0.5 * (1.0 + g); - a->b1 = -0.5 * (1.0 + g); - a->a1 = -g; -} - -SPHP* SPHP::create_sphp(int run, int size, float* in, float* out, double rate, double fc, int nstages) -{ - SPHP *a = new SPHP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->fc = fc; - a->nstages = nstages; - calc_sphp(a); - return a; -} - -void SPHP::decalc_sphp(SPHP *a) -{ - delete[](a->y1); - delete[](a->y0); - delete[](a->x1); - delete[](a->x0); -} - -void SPHP::destroy_sphp(SPHP *a) -{ - decalc_sphp(a); - delete(a); -} - -void SPHP::flush_sphp(SPHP *a) -{ - std::fill(a->x0, a->x0 + a->nstages * 2, 0); - std::fill(a->x1, a->x0 + a->nstages * 2, 0); - std::fill(a->y0, a->x0 + a->nstages * 2, 0); - std::fill(a->y1, a->x0 + a->nstages * 2, 0); -} - -void SPHP::xsphp(SPHP *a) -{ - if (a->run) - { - int i, j, n; - for (i = 0; i < a->size; i++) - { - for (j = 0; j < 2; j++) - { - a->x0[j] = a->in[2 * i + j]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; - - a->y0[2 * n + j] = a->b0 * a->x0[2 * n + j] - + a->b1 * a->x1[2 * n + j] - - a->a1 * a->y1[2 * n + j]; - a->y1[2 * n + j] = a->y0[2 * n + j]; - a->x1[2 * n + j] = a->x0[2 * n + j]; - } - - a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; - } - } - } - else if (a->out != a->in) - { - std::copy(a->in, a->in + a->size * 2, a->out); - } -} - -void SPHP::setBuffers_sphp(SPHP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void SPHP::setSamplerate_sphp(SPHP *a, int rate) -{ - decalc_sphp(a); - a->rate = rate; - calc_sphp(a); -} - -void SPHP::setSize_sphp(SPHP *a, int size) -{ - a->size = size; - flush_sphp(a); -} - -/******************************************************************************************************** -* * -* Double Single-Pole High-Pass * -* * -********************************************************************************************************/ - -void SPHP::calc_dsphp(SPHP *a) -{ - double g; - a->x0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->x1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y0 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - a->y1 = new double[a->nstages]; // (float*)malloc0(a->nstages * sizeof(float)); - g = exp(-TWOPI * a->fc / a->rate); - a->b0 = +0.5 * (1.0 + g); - a->b1 = -0.5 * (1.0 + g); - a->a1 = -g; -} - -SPHP* SPHP::create_dsphp(int run, int size, float* in, float* out, double rate, double fc, int nstages) -{ - SPHP *a = new SPHP; - a->run = run; - a->size = size; - a->in = in; - a->out = out; - a->rate = rate; - a->fc = fc; - a->nstages = nstages; - calc_dsphp(a); - return a; -} - -void SPHP::decalc_dsphp(SPHP *a) -{ - delete[](a->y1); - delete[](a->y0); - delete[](a->x1); - delete[](a->x0); -} - -void SPHP::destroy_dsphp(SPHP *a) -{ - decalc_dsphp(a); - delete(a); -} - -void SPHP::flush_dsphp(SPHP *a) -{ - memset(a->x0, 0, a->nstages * sizeof(float)); - memset(a->x1, 0, a->nstages * sizeof(float)); - memset(a->y0, 0, a->nstages * sizeof(float)); - memset(a->y1, 0, a->nstages * sizeof(float)); -} - -void SPHP::xdsphp(SPHP *a) -{ - if (a->run) - { - int i, n; - - for (i = 0; i < a->size; i++) - { - a->x0[0] = a->in[i]; - - for (n = 0; n < a->nstages; n++) - { - if (n > 0) - a->x0[n] = a->y0[n - 1]; - - a->y0[n] = a->b0 * a->x0[n] - + a->b1 * a->x1[n] - - a->a1 * a->y1[n]; - a->y1[n] = a->y0[n]; - a->x1[n] = a->x0[n]; - } - - a->out[i] = a->y0[a->nstages - 1]; - } - } - else if (a->out != a->in) - { - memcpy(a->out, a->in, a->size * sizeof(float)); - } -} - -void SPHP::setBuffers_dsphp(SPHP *a, float* in, float* out) -{ - a->in = in; - a->out = out; -} - -void SPHP::setSamplerate_dsphp(SPHP *a, int rate) -{ - decalc_dsphp(a); - a->rate = rate; - calc_dsphp(a); -} - -void SPHP::setSize_dsphp(SPHP *a, int size) -{ - a->size = size; - flush_dsphp(a); -} - -} // namespace WDSP diff --git a/wdsp/iir.hpp b/wdsp/iir.hpp deleted file mode 100644 index d6ba5cfce..000000000 --- a/wdsp/iir.hpp +++ /dev/null @@ -1,440 +0,0 @@ -/* iir.h - -This file is part of a program that implements a Software-Defined Radio. - -Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V -Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel - -This program is free software; you can redistribute it and/or -modify it under the terms of the GNU General Public License -as published by the Free Software Foundation; either version 2 -of the License, or (at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. - -The author can be reached by email at - -warren@wpratt.com - -*/ - -/******************************************************************************************************** -* * -* Bi-Quad Notch * -* * -********************************************************************************************************/ - -#ifndef wdsp_snotch_h -#define wdsp_snotch_h - -#include "export.h" - -namespace WDSP { - -class WDSP_API SNOTCH -{ -public: - int run; - int size; - float* in; - float* out; - double rate; - double f; - double bw; - double a0, a1, a2, b1, b2; - double x0, x1, x2, y1, y2; - - static SNOTCH* create_snotch (int run, int size, float* in, float* out, int rate, double f, double bw); - static void destroy_snotch (SNOTCH *a); - static void flush_snotch (SNOTCH *a); - static void xsnotch (SNOTCH *a); - static void setBuffers_snotch (SNOTCH *a, float* in, float* out); - static void setSamplerate_snotch (SNOTCH *a, int rate); - static void setSize_snotch (SNOTCH *a, int size); - static void SetSNCTCSSFreq (SNOTCH *a, double freq); - static void SetSNCTCSSRun (SNOTCH *a, int run); - -private: - static void calc_snotch (SNOTCH *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Complex Bi-Quad Peaking * -* * -********************************************************************************************************/ - -#ifndef wdsp_speak_h -#define wdsp_speak_h - -#include "export.h" - -namespace WDSP { - -class RXA; - -class WDSP_API SPEAK -{ -public: - int run; - int size; - float* in; - float* out; - double rate; - double f; - double bw; - double cbw; - double gain; - double fgain; - int nstages; - int design; - double a0, a1, a2, b1, b2; - double *x0, *x1, *x2, *y0, *y1, *y2; - - static SPEAK* create_speak ( - int run, - int size, - float* in, - float* out, - int rate, - double f, - double bw, - double gain, - int nstages, - int design - ); - static void destroy_speak (SPEAK *a); - static void flush_speak (SPEAK *a); - static void xspeak (SPEAK *a); - static void setBuffers_speak (SPEAK *a, float* in, float* out); - static void setSamplerate_speak (SPEAK *a, int rate); - static void setSize_speak (SPEAK *a, int size); - // RXA - static void SetSPCWRun (RXA& rxa, int run); - static void SetSPCWFreq (RXA& rxa, double freq); - static void SetSPCWBandwidth (RXA& rxa, double bw); - static void SetSPCWGain (RXA& rxa, double gain); - static void calc_speak (SPEAK *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Complex Multiple Peaking * -* * -********************************************************************************************************/ - -#ifndef _mpeak_h -#define _mpeak_h - -#include "export.h" - -namespace WDSP { - -class RXA; - -class WDSP_API MPEAK -{ -public: - int run; - int size; - float* in; - float* out; - int rate; - int npeaks; - int* enable; - double* f; - double* bw; - double* gain; - int nstages; - SPEAK** pfil; - float* tmp; - float* mix; - - static MPEAK* create_mpeak ( - int run, - int size, - float* in, - float* out, - int rate, - int npeaks, - int* enable, - double* f, - double* bw, - double* gain, - int nstages - ); - static void destroy_mpeak (MPEAK *a); - static void flush_mpeak (MPEAK *a); - static void xmpeak (MPEAK *a); - static void setBuffers_mpeak (MPEAK *a, float* in, float* out); - static void setSamplerate_mpeak (MPEAK *a, int rate); - static void setSize_mpeak (MPEAK *a, int size); - // RXA - static void SetmpeakRun (RXA& rxa, int run); - static void SetmpeakNpeaks (RXA& rxa, int npeaks); - static void SetmpeakFilEnable (RXA& rxa, int fil, int enable); - static void SetmpeakFilFreq (RXA& rxa, int fil, double freq); - static void SetmpeakFilBw (RXA& rxa, int fil, double bw); - static void SetmpeakFilGain (RXA& rxa, int fil, double gain); - -private: - static void calc_mpeak (MPEAK *a); - static void decalc_mpeak (MPEAK *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Phase Rotator * -* * -********************************************************************************************************/ - -#ifndef wdsp_phrot_h -#define wdsp_phrot_h - -#include "export.h" - -namespace WDSP { - -class TXA; - -class WDSP_API PHROT -{ -public: - int reverse; - int run; - int size; - float* in; - float* out; - int rate; - double fc; - int nstages; - // normalized such that a0 = 1 - double a1, b0, b1; - double *x0, *x1, *y0, *y1; - - static PHROT* create_phrot (int run, int size, float* in, float* out, int rate, double fc, int nstages); - static void destroy_phrot (PHROT *a); - static void flush_phrot (PHROT *a); - static void xphrot (PHROT *a); - static void setBuffers_phrot (PHROT *a, float* in, float* out); - static void setSamplerate_phrot (PHROT *a, int rate); - static void setSize_phrot (PHROT *a, int size); - // TXA Properties - static void SetPHROTRun (TXA& txa, int run); - static void SetPHROTCorner (TXA& txa, double corner); - static void SetPHROTNstages (TXA& txa, int nstages); - static void SetPHROTReverse (TXA& txa, int reverse); - -private: - static void calc_phrot (PHROT *a); - static void decalc_phrot (PHROT *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Complex Bi-Quad Low-Pass * -* * -********************************************************************************************************/ - -#ifndef wdsp_bqlp_h -#define wdsp_bqlp_h - -#include "export.h" - -namespace WDSP { - -class WDSP_API BQLP -{ -public: - int run; - int size; - float* in; - float* out; - double rate; - double fc; - double Q; - double gain; - int nstages; - double a0, a1, a2, b1, b2; - double* x0, * x1, * x2, * y0, * y1, * y2; - - static BQLP* create_bqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages); - static void destroy_bqlp(BQLP *a); - static void flush_bqlp(BQLP *a); - static void xbqlp(BQLP *a); - static void setBuffers_bqlp(BQLP *a, float* in, float* out); - static void setSamplerate_bqlp(BQLP *a, int rate); - static void setSize_bqlp(BQLP *a, int size); - -private: - static void calc_bqlp(BQLP *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Double Bi-Quad Low-Pass * -* * -********************************************************************************************************/ - -#ifndef wdsp_dbqlp_h -#define wdsp_dbqlp_h - -#include "export.h" - -namespace WDSP { - -class WDSP_API DBQLP -{ -public: - static BQLP* create_dbqlp(int run, int size, float* in, float* out, double rate, double fc, double Q, double gain, int nstages); - static void destroy_dbqlp(BQLP *a); - static void flush_dbqlp(BQLP *a); - static void xdbqlp(BQLP *a); - static void setBuffers_dbqlp(BQLP *a, float* in, float* out); - static void setSamplerate_dbqlp(BQLP *a, int rate); - static void setSize_dbqlp(BQLP *a, int size); - -private: - static void calc_dbqlp(BQLP *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Complex Bi-Quad Band-Pass * -* * -********************************************************************************************************/ - -#ifndef wdsp_bqbp_h -#define wdsp_bqbp_h - -#include "export.h" - -namespace WDSP { - -class WDSP_API BQBP -{ -public: - int run; - int size; - float* in; - float* out; - double rate; - double f_low; - double f_high; - double gain; - int nstages; - double a0, a1, a2, b1, b2; - double* x0, * x1, * x2, * y0, * y1, * y2; - - static BQBP* create_bqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages); - static void destroy_bqbp(BQBP *a); - static void flush_bqbp(BQBP *a); - static void xbqbp(BQBP *a); - static void setBuffers_bqbp(BQBP *a, float* in, float* out); - static void setSamplerate_bqbp(BQBP *a, int rate); - static void setSize_bqbp(BQBP *a, int size); - - // Double Bi-Quad Band-Pass - static BQBP* create_dbqbp(int run, int size, float* in, float* out, double rate, double f_low, double f_high, double gain, int nstages); - static void destroy_dbqbp(BQBP *a); - static void flush_dbqbp(BQBP *a); - static void xdbqbp(BQBP *a); - static void setBuffers_dbqbp(BQBP *a, float* in, float* out); - static void setSamplerate_dbqbp(BQBP *a, int rate); - static void setSize_dbqbp(BQBP *a, int size); - -private: - static void calc_bqbp(BQBP *a); - static void calc_dbqbp(BQBP *a); -}; - -} // namespace WDSP - -#endif - -/******************************************************************************************************** -* * -* Double Single-Pole High-Pass * -* * -********************************************************************************************************/ - -#ifndef wdsp_dsphp_h -#define wdsp_dsphp_h - -#include "export.h" - -namespace WDSP { - -class WDSP_API SPHP -{ -public: - int run; - int size; - float* in; - float* out; - double rate; - double fc; - int nstages; - double a1, b0, b1; - double* x0, * x1, * y0, * y1; - - static SPHP* create_dsphp(int run, int size, float* in, float* out, double rate, double fc, int nstages); - static void destroy_dsphp(SPHP *a); - static void flush_dsphp(SPHP *a); - static void xdsphp(SPHP *a); - static void setBuffers_dsphp(SPHP *a, float* in, float* out); - static void setSamplerate_dsphp(SPHP *a, int rate); - static void setSize_dsphp(SPHP *a, int size); - - // Complex Single-Pole High-Pass - static SPHP* create_sphp(int run, int size, float* in, float* out, double rate, double fc, int nstages); - static void destroy_sphp(SPHP *a); - static void flush_sphp(SPHP *a); - static void xsphp(SPHP *a); - static void setBuffers_sphp(SPHP *a, float* in, float* out); - static void setSamplerate_sphp(SPHP *a, int rate); - static void setSize_sphp(SPHP *a, int size); - -private: - static void calc_sphp(SPHP *a); - static void decalc_sphp(SPHP *a); - static void calc_dsphp(SPHP *a); - static void decalc_dsphp(SPHP *a); -}; - -} // namespace WDSP - -#endif - - diff --git a/wdsp/mpeak.cpp b/wdsp/mpeak.cpp new file mode 100644 index 000000000..6c91e94c5 --- /dev/null +++ b/wdsp/mpeak.cpp @@ -0,0 +1,221 @@ +/* mpeak.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "mpeak.hpp" +#include "speak.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Complex Multiple Peaking * +* * +********************************************************************************************************/ + +void MPEAK::calc_mpeak (MPEAK *a) +{ + int i; + a->tmp = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex)); + a->mix = new float[a->size * 2]; // (float *) malloc0 (a->size * sizeof (complex)); + for (i = 0; i < a->npeaks; i++) + { + a->pfil[i] = SPEAK::create_speak ( + 1, + a->size, + a->in, + a->tmp, + a->rate, + a->f[i], + a->bw[i], + a->gain[i], + a->nstages, + 1 + ); + } +} + +void MPEAK::decalc_mpeak (MPEAK *a) +{ + int i; + for (i = 0; i < a->npeaks; i++) + SPEAK::destroy_speak (a->pfil[i]); + delete[] (a->mix); + delete[] (a->tmp); +} + +MPEAK* MPEAK::create_mpeak ( + int run, + int size, + float* in, + float* out, + int rate, + int npeaks, + int* enable, + double* f, + double* bw, + double* gain, + int nstages +) +{ + MPEAK *a = new MPEAK; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->npeaks = npeaks; + a->nstages = nstages; + a->enable = new int[a->npeaks]; // (int *) malloc0 (a->npeaks * sizeof (int)); + a->f = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); + a->bw = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); + a->gain = new double[a->npeaks]; // (float *) malloc0 (a->npeaks * sizeof (float)); + memcpy (a->enable, enable, a->npeaks * sizeof (int)); + memcpy (a->f, f, a->npeaks * sizeof (double)); + memcpy (a->bw, bw, a->npeaks * sizeof (double)); + memcpy (a->gain, gain, a->npeaks * sizeof (double)); + a->pfil = new SPEAK*[a->npeaks]; // (SPEAK *) malloc0 (a->npeaks * sizeof (SPEAK)); + calc_mpeak (a); + return a; +} + +void MPEAK::destroy_mpeak (MPEAK *a) +{ + decalc_mpeak (a); + delete[] (a->pfil); + delete[] (a->gain); + delete[] (a->bw); + delete[] (a->f); + delete[] (a->enable); + delete (a); +} + +void MPEAK::flush_mpeak (MPEAK *a) +{ + int i; + for (i = 0; i < a->npeaks; i++) + SPEAK::flush_speak (a->pfil[i]); +} + +void MPEAK::xmpeak (MPEAK *a) +{ + if (a->run) + { + int i, j; + std::fill(a->mix, a->mix + a->size * 2, 0); + + for (i = 0; i < a->npeaks; i++) + { + if (a->enable[i]) + { + SPEAK::xspeak (a->pfil[i]); + for (j = 0; j < 2 * a->size; j++) + a->mix[j] += a->tmp[j]; + } + } + + std::copy(a->mix, a->mix + a->size * 2, a->out); + } + else if (a->in != a->out) + { + std::copy( a->in, a->in + a->size * 2, a->out); + } +} + +void MPEAK::setBuffers_mpeak (MPEAK *a, float* in, float* out) +{ + decalc_mpeak (a); + a->in = in; + a->out = out; + calc_mpeak (a); +} + +void MPEAK::setSamplerate_mpeak (MPEAK *a, int rate) +{ + decalc_mpeak (a); + a->rate = rate; + calc_mpeak (a); +} + +void MPEAK::setSize_mpeak (MPEAK *a, int size) +{ + decalc_mpeak (a); + a->size = size; + calc_mpeak (a); +} + +/******************************************************************************************************** +* * +* RXA Properties * +* * +********************************************************************************************************/ + +void MPEAK::SetmpeakRun (RXA& rxa, int run) +{ + MPEAK *a = rxa.mpeak; + a->run = run; +} + +void MPEAK::SetmpeakNpeaks (RXA& rxa, int npeaks) +{ + MPEAK *a = rxa.mpeak; + a->npeaks = npeaks; +} + +void MPEAK::SetmpeakFilEnable (RXA& rxa, int fil, int enable) +{ + MPEAK *a = rxa.mpeak; + a->enable[fil] = enable; +} + +void MPEAK::SetmpeakFilFreq (RXA& rxa, int fil, double freq) +{ + MPEAK *a = rxa.mpeak; + a->f[fil] = freq; + a->pfil[fil]->f = freq; + SPEAK::calc_speak(a->pfil[fil]); +} + +void MPEAK::SetmpeakFilBw (RXA& rxa, int fil, double bw) +{ + MPEAK *a = rxa.mpeak; + a->bw[fil] = bw; + a->pfil[fil]->bw = bw; + SPEAK::calc_speak(a->pfil[fil]); +} + +void MPEAK::SetmpeakFilGain (RXA& rxa, int fil, double gain) +{ + MPEAK *a = rxa.mpeak; + a->gain[fil] = gain; + a->pfil[fil]->gain = gain; + SPEAK::calc_speak(a->pfil[fil]); +} + +} // namespace WDSP diff --git a/wdsp/mpeak.hpp b/wdsp/mpeak.hpp new file mode 100644 index 000000000..6a88f631a --- /dev/null +++ b/wdsp/mpeak.hpp @@ -0,0 +1,96 @@ +/* mpeak.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Complex Multiple Peaking * +* * +********************************************************************************************************/ + +#ifndef _mpeak_h +#define _mpeak_h + +#include "export.h" + +namespace WDSP { + +class RXA; +class SPEAK; + +class WDSP_API MPEAK +{ +public: + int run; + int size; + float* in; + float* out; + int rate; + int npeaks; + int* enable; + double* f; + double* bw; + double* gain; + int nstages; + SPEAK** pfil; + float* tmp; + float* mix; + + static MPEAK* create_mpeak ( + int run, + int size, + float* in, + float* out, + int rate, + int npeaks, + int* enable, + double* f, + double* bw, + double* gain, + int nstages + ); + static void destroy_mpeak (MPEAK *a); + static void flush_mpeak (MPEAK *a); + static void xmpeak (MPEAK *a); + static void setBuffers_mpeak (MPEAK *a, float* in, float* out); + static void setSamplerate_mpeak (MPEAK *a, int rate); + static void setSize_mpeak (MPEAK *a, int size); + // RXA + static void SetmpeakRun (RXA& rxa, int run); + static void SetmpeakNpeaks (RXA& rxa, int npeaks); + static void SetmpeakFilEnable (RXA& rxa, int fil, int enable); + static void SetmpeakFilFreq (RXA& rxa, int fil, double freq); + static void SetmpeakFilBw (RXA& rxa, int fil, double bw); + static void SetmpeakFilGain (RXA& rxa, int fil, double gain); + +private: + static void calc_mpeak (MPEAK *a); + static void decalc_mpeak (MPEAK *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/phrot.cpp b/wdsp/phrot.cpp new file mode 100644 index 000000000..a233db27d --- /dev/null +++ b/wdsp/phrot.cpp @@ -0,0 +1,182 @@ +/* phrot.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "phrot.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Phase Rotator * +* * +********************************************************************************************************/ + +void PHROT::calc_phrot (PHROT *a) +{ + double g; + a->x0 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); + a->x1 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); + a->y0 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); + a->y1 = new double[a->nstages]; // (float *) malloc0 (a->nstages * sizeof (float)); + g = tan (PI * a->fc / (float)a->rate); + a->b0 = (g - 1.0) / (g + 1.0); + a->b1 = 1.0; + a->a1 = a->b0; +} + +void PHROT::decalc_phrot (PHROT *a) +{ + delete[] (a->y1); + delete[] (a->y0); + delete[] (a->x1); + delete[] (a->x0); +} + +PHROT* PHROT::create_phrot (int run, int size, float* in, float* out, int rate, double fc, int nstages) +{ + PHROT *a = new PHROT; + a->reverse = 0; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->fc = fc; + a->nstages = nstages; + calc_phrot (a); + return a; +} + +void PHROT::destroy_phrot (PHROT *a) +{ + decalc_phrot (a); + delete (a); +} + +void PHROT::flush_phrot (PHROT *a) +{ + memset (a->x0, 0, a->nstages * sizeof (double)); + memset (a->x1, 0, a->nstages * sizeof (double)); + memset (a->y0, 0, a->nstages * sizeof (double)); + memset (a->y1, 0, a->nstages * sizeof (double)); +} + +void PHROT::xphrot (PHROT *a) +{ + if (a->reverse) + { + for (int i = 0; i < a->size; i++) + a->in[2 * i + 0] = -a->in[2 * i + 0]; + } + + if (a->run) + { + int i, n; + + for (i = 0; i < a->size; i++) + { + a->x0[0] = a->in[2 * i + 0]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) a->x0[n] = a->y0[n - 1]; + a->y0[n] = a->b0 * a->x0[n] + + a->b1 * a->x1[n] + - a->a1 * a->y1[n]; + a->y1[n] = a->y0[n]; + a->x1[n] = a->x0[n]; + } + + a->out[2 * i + 0] = a->y0[a->nstages - 1]; + } + } + else if (a->out != a->in) + { + std::copy( a->in, a->in + a->size * 2, a->out); + } +} + +void PHROT::setBuffers_phrot (PHROT *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void PHROT::setSamplerate_phrot (PHROT *a, int rate) +{ + decalc_phrot (a); + a->rate = rate; + calc_phrot (a); +} + +void PHROT::setSize_phrot (PHROT *a, int size) +{ + a->size = size; + flush_phrot (a); +} + +/******************************************************************************************************** +* * +* TXA Properties * +* * +********************************************************************************************************/ + +void PHROT::SetPHROTRun (TXA& txa, int run) +{ + PHROT *a = txa.phrot; + a->run = run; + + if (a->run) + flush_phrot (a); +} + +void PHROT::SetPHROTCorner (TXA& txa, double corner) +{ + PHROT *a = txa.phrot; + decalc_phrot (a); + a->fc = corner; + calc_phrot (a); +} + +void PHROT::SetPHROTNstages (TXA& txa, int nstages) +{ + PHROT *a = txa.phrot; + decalc_phrot (a); + a->nstages = nstages; + calc_phrot (a); +} + +void PHROT::SetPHROTReverse (TXA& txa, int reverse) +{ + PHROT *a = txa.phrot; + a->reverse = reverse; +} + +} // namespace WDSP diff --git a/wdsp/phrot.hpp b/wdsp/phrot.hpp new file mode 100644 index 000000000..ef0f2dbf3 --- /dev/null +++ b/wdsp/phrot.hpp @@ -0,0 +1,78 @@ +/* phrot.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Phase Rotator * +* * +********************************************************************************************************/ + +#ifndef wdsp_phrot_h +#define wdsp_phrot_h + +#include "export.h" + +namespace WDSP { + +class TXA; + +class WDSP_API PHROT +{ +public: + int reverse; + int run; + int size; + float* in; + float* out; + int rate; + double fc; + int nstages; + // normalized such that a0 = 1 + double a1, b0, b1; + double *x0, *x1, *y0, *y1; + + static PHROT* create_phrot (int run, int size, float* in, float* out, int rate, double fc, int nstages); + static void destroy_phrot (PHROT *a); + static void flush_phrot (PHROT *a); + static void xphrot (PHROT *a); + static void setBuffers_phrot (PHROT *a, float* in, float* out); + static void setSamplerate_phrot (PHROT *a, int rate); + static void setSize_phrot (PHROT *a, int size); + // TXA Properties + static void SetPHROTRun (TXA& txa, int run); + static void SetPHROTCorner (TXA& txa, double corner); + static void SetPHROTNstages (TXA& txa, int nstages); + static void SetPHROTReverse (TXA& txa, int reverse); + +private: + static void calc_phrot (PHROT *a); + static void decalc_phrot (PHROT *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/snotch.cpp b/wdsp/snotch.cpp new file mode 100644 index 000000000..8e799e6c4 --- /dev/null +++ b/wdsp/snotch.cpp @@ -0,0 +1,136 @@ +/* iir.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "snotch.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Bi-Quad Notch * +* * +********************************************************************************************************/ + +void SNOTCH::calc_snotch (SNOTCH *a) +{ + double fn, qk, qr, csn; + fn = a->f / (float)a->rate; + csn = cos (TWOPI * fn); + qr = 1.0 - 3.0 * a->bw; + qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn)); + a->a0 = + qk; + a->a1 = - 2.0 * qk * csn; + a->a2 = + qk; + a->b1 = + 2.0 * qr * csn; + a->b2 = - qr * qr; + flush_snotch (a); +} + +SNOTCH* SNOTCH::create_snotch (int run, int size, float* in, float* out, int rate, double f, double bw) +{ + SNOTCH *a = new SNOTCH; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->f = f; + a->bw = bw; + calc_snotch (a); + return a; +} + +void SNOTCH::destroy_snotch (SNOTCH *a) +{ + delete (a); +} + +void SNOTCH::flush_snotch (SNOTCH *a) +{ + a->x1 = a->x2 = a->y1 = a->y2 = 0.0; +} + +void SNOTCH::xsnotch (SNOTCH *a) +{ + if (a->run) + { + int i; + for (i = 0; i < a->size; i++) + { + a->x0 = a->in[2 * i + 0]; + a->out[2 * i + 0] = a->a0 * a->x0 + a->a1 * a->x1 + a->a2 * a->x2 + a->b1 * a->y1 + a->b2 * a->y2; + a->y2 = a->y1; + a->y1 = a->out[2 * i + 0]; + a->x2 = a->x1; + a->x1 = a->x0; + } + } + else if (a->out != a->in) + { + std::copy( a->in, a->in + a->size * 2, a->out); + } +} + +void SNOTCH::setBuffers_snotch (SNOTCH *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void SNOTCH::setSamplerate_snotch (SNOTCH *a, int rate) +{ + a->rate = rate; + calc_snotch (a); +} + +void SNOTCH::setSize_snotch (SNOTCH *a, int size) +{ + a->size = size; + flush_snotch (a); +} + +/******************************************************************************************************** +* * +* RXA Properties * +* * +********************************************************************************************************/ + +void SNOTCH::SetSNCTCSSFreq (SNOTCH *a, double freq) +{ + a->f = freq; + calc_snotch (a); +} + +void SNOTCH::SetSNCTCSSRun (SNOTCH *a, int run) +{ + a->run = run; +} + +} // namespace WDSP diff --git a/wdsp/snotch.hpp b/wdsp/snotch.hpp new file mode 100644 index 000000000..39c917a8d --- /dev/null +++ b/wdsp/snotch.hpp @@ -0,0 +1,70 @@ +/* snotch.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Bi-Quad Notch * +* * +********************************************************************************************************/ + +#ifndef wdsp_snotch_h +#define wdsp_snotch_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API SNOTCH +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double f; + double bw; + double a0, a1, a2, b1, b2; + double x0, x1, x2, y1, y2; + + static SNOTCH* create_snotch (int run, int size, float* in, float* out, int rate, double f, double bw); + static void destroy_snotch (SNOTCH *a); + static void flush_snotch (SNOTCH *a); + static void xsnotch (SNOTCH *a); + static void setBuffers_snotch (SNOTCH *a, float* in, float* out); + static void setSamplerate_snotch (SNOTCH *a, int rate); + static void setSize_snotch (SNOTCH *a, int size); + static void SetSNCTCSSFreq (SNOTCH *a, double freq); + static void SetSNCTCSSRun (SNOTCH *a, int run); + +private: + static void calc_snotch (SNOTCH *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/speak.cpp b/wdsp/speak.cpp new file mode 100644 index 000000000..a4b5a461a --- /dev/null +++ b/wdsp/speak.cpp @@ -0,0 +1,260 @@ +/* speak.cpp + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "comm.hpp" +#include "speak.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Complex Bi-Quad Peaking * +* * +********************************************************************************************************/ + +void SPEAK::calc_speak (SPEAK *a) +{ + double ratio; + double f_corr, g_corr, bw_corr, bw_parm, A, f_min; + + switch (a->design) + { + case 0: + ratio = a->bw / a->f; + switch (a->nstages) + { + case 4: + bw_parm = 2.4; + f_corr = 1.0 - 0.160 * ratio + 1.440 * ratio * ratio; + g_corr = 1.0 - 1.003 * ratio + 3.990 * ratio * ratio; + break; + default: + bw_parm = 1.0; + f_corr = 1.0; + g_corr = 1.0; + break; + } + { + double fn, qk, qr, csn; + a->fgain = a->gain / g_corr; + fn = a->f / (double)a->rate / f_corr; + csn = cos (TWOPI * fn); + qr = 1.0 - 3.0 * a->bw / (double)a->rate * bw_parm; + qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn)); + a->a0 = 1.0 - qk; + a->a1 = 2.0 * (qk - qr) * csn; + a->a2 = qr * qr - qk; + a->b1 = 2.0 * qr * csn; + a->b2 = - qr * qr; + } + break; + + case 1: + if (a->f < 200.0) a->f = 200.0; + ratio = a->bw / a->f; + switch (a->nstages) + { + case 4: + bw_parm = 5.0; + bw_corr = 1.13 * ratio - 0.956 * ratio * ratio; + A = 2.5; + f_min = 50.0; + break; + default: + bw_parm = 1.0; + bw_corr = 1.0; + g_corr = 1.0; + A = 2.5; + f_min = 50.0; + break; + } + { + double w0, sn, c, den; + if (a->f < f_min) a->f = f_min; + w0 = TWOPI * a->f / (double)a->rate; + sn = sin (w0); + a->cbw = bw_corr * a->f; + c = sn * sinh(0.5 * log((a->f + 0.5 * a->cbw * bw_parm) / (a->f - 0.5 * a->cbw * bw_parm)) * w0 / sn); + den = 1.0 + c / A; + a->a0 = (1.0 + c * A) / den; + a->a1 = - 2.0 * cos (w0) / den; + a->a2 = (1 - c * A) / den; + a->b1 = - a->a1; + a->b2 = - (1 - c / A ) / den; + a->fgain = a->gain / pow (A * A, (double)a->nstages); + } + break; + } + flush_speak (a); +} + +SPEAK* SPEAK::create_speak ( + int run, + int size, + float* in, + float* out, + int rate, + double f, + double bw, + double gain, + int nstages, + int design +) +{ + SPEAK *a = new SPEAK; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->f = f; + a->bw = bw; + a->gain = gain; + a->nstages = nstages; + a->design = design; + a->x0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); + a->x1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); + a->x2 = new double[a->nstages * 2]; //(float *) malloc0 (a->nstages * sizeof (complex)); + a->y0 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); + a->y1 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); + a->y2 = new double[a->nstages * 2]; // (float *) malloc0 (a->nstages * sizeof (complex)); + calc_speak (a); + return a; +} + +void SPEAK::destroy_speak (SPEAK *a) +{ + delete[] (a->y2); + delete[] (a->y1); + delete[] (a->y0); + delete[] (a->x2); + delete[] (a->x1); + delete[] (a->x0); + delete (a); +} + +void SPEAK::flush_speak (SPEAK *a) +{ + int i; + for (i = 0; i < a->nstages; i++) + { + a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0; + a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0; + } +} + +void SPEAK::xspeak (SPEAK *a) +{ + if (a->run) + { + int i, j, n; + + for (i = 0; i < a->size; i++) + { + for (j = 0; j < 2; j++) + { + a->x0[j] = a->fgain * a->in[2 * i + j]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; + a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j] + + a->a1 * a->x1[2 * n + j] + + a->a2 * a->x2[2 * n + j] + + a->b1 * a->y1[2 * n + j] + + a->b2 * a->y2[2 * n + j]; + a->y2[2 * n + j] = a->y1[2 * n + j]; + a->y1[2 * n + j] = a->y0[2 * n + j]; + a->x2[2 * n + j] = a->x1[2 * n + j]; + a->x1[2 * n + j] = a->x0[2 * n + j]; + } + + a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; + } + } + } + else if (a->out != a->in) + { + std::copy( a->in, a->in + a->size * 2, a->out); + } +} + +void SPEAK::setBuffers_speak (SPEAK *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void SPEAK::setSamplerate_speak (SPEAK *a, int rate) +{ + a->rate = rate; + calc_speak (a); +} + +void SPEAK::setSize_speak (SPEAK *a, int size) +{ + a->size = size; + flush_speak (a); +} + +/******************************************************************************************************** +* * +* RXA Properties * +* * +********************************************************************************************************/ + +void SPEAK::SetSPCWRun (RXA& rxa, int run) +{ + SPEAK *a = rxa.speak; + a->run = run; +} + +void SPEAK::SetSPCWFreq (RXA& rxa, double freq) +{ + SPEAK *a = rxa.speak; + a->f = freq; + calc_speak (a); +} + +void SPEAK::SetSPCWBandwidth (RXA& rxa, double bw) +{ + SPEAK *a = rxa.speak; + a->bw = bw; + calc_speak (a); +} + +void SPEAK::SetSPCWGain (RXA& rxa, double gain) +{ + SPEAK *a = rxa.speak; + a->gain = gain; + calc_speak (a); +} + +} // namespace WDSP diff --git a/wdsp/speak.hpp b/wdsp/speak.hpp new file mode 100644 index 000000000..9642d0ad1 --- /dev/null +++ b/wdsp/speak.hpp @@ -0,0 +1,89 @@ +/* speak.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Complex Bi-Quad Peaking * +* * +********************************************************************************************************/ + +#ifndef wdsp_speak_h +#define wdsp_speak_h + +#include "export.h" + +namespace WDSP { + +class RXA; + +class WDSP_API SPEAK +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double f; + double bw; + double cbw; + double gain; + double fgain; + int nstages; + int design; + double a0, a1, a2, b1, b2; + double *x0, *x1, *x2, *y0, *y1, *y2; + + static SPEAK* create_speak ( + int run, + int size, + float* in, + float* out, + int rate, + double f, + double bw, + double gain, + int nstages, + int design + ); + static void destroy_speak (SPEAK *a); + static void flush_speak (SPEAK *a); + static void xspeak (SPEAK *a); + static void setBuffers_speak (SPEAK *a, float* in, float* out); + static void setSamplerate_speak (SPEAK *a, int rate); + static void setSize_speak (SPEAK *a, int size); + // RXA + static void SetSPCWRun (RXA& rxa, int run); + static void SetSPCWFreq (RXA& rxa, double freq); + static void SetSPCWBandwidth (RXA& rxa, double bw); + static void SetSPCWGain (RXA& rxa, double gain); + static void calc_speak (SPEAK *a); +}; + +} // namespace WDSP + +#endif diff --git a/wdsp/sphp.cpp b/wdsp/sphp.cpp new file mode 100644 index 000000000..88045dfea --- /dev/null +++ b/wdsp/sphp.cpp @@ -0,0 +1,141 @@ +/* iir.c + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +#include "sphp.hpp" +#include "RXA.hpp" +#include "TXA.hpp" + +namespace WDSP { + +/******************************************************************************************************** +* * +* Complex Single-Pole High-Pass * +* * +********************************************************************************************************/ + +void SPHP::calc_sphp(SPHP *a) +{ + double g; + a->x0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->x1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y0 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + a->y1 = new double[a->nstages * 2]; // (float*)malloc0(a->nstages * sizeof(complex)); + g = exp(-TWOPI * a->fc / a->rate); + a->b0 = +0.5 * (1.0 + g); + a->b1 = -0.5 * (1.0 + g); + a->a1 = -g; +} + +SPHP* SPHP::create_sphp(int run, int size, float* in, float* out, double rate, double fc, int nstages) +{ + SPHP *a = new SPHP; + a->run = run; + a->size = size; + a->in = in; + a->out = out; + a->rate = rate; + a->fc = fc; + a->nstages = nstages; + calc_sphp(a); + return a; +} + +void SPHP::decalc_sphp(SPHP *a) +{ + delete[](a->y1); + delete[](a->y0); + delete[](a->x1); + delete[](a->x0); +} + +void SPHP::destroy_sphp(SPHP *a) +{ + decalc_sphp(a); + delete(a); +} + +void SPHP::flush_sphp(SPHP *a) +{ + std::fill(a->x0, a->x0 + a->nstages * 2, 0); + std::fill(a->x1, a->x0 + a->nstages * 2, 0); + std::fill(a->y0, a->x0 + a->nstages * 2, 0); + std::fill(a->y1, a->x0 + a->nstages * 2, 0); +} + +void SPHP::xsphp(SPHP *a) +{ + if (a->run) + { + int i, j, n; + for (i = 0; i < a->size; i++) + { + for (j = 0; j < 2; j++) + { + a->x0[j] = a->in[2 * i + j]; + + for (n = 0; n < a->nstages; n++) + { + if (n > 0) + a->x0[2 * n + j] = a->y0[2 * (n - 1) + j]; + + a->y0[2 * n + j] = a->b0 * a->x0[2 * n + j] + + a->b1 * a->x1[2 * n + j] + - a->a1 * a->y1[2 * n + j]; + a->y1[2 * n + j] = a->y0[2 * n + j]; + a->x1[2 * n + j] = a->x0[2 * n + j]; + } + + a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j]; + } + } + } + else if (a->out != a->in) + { + std::copy(a->in, a->in + a->size * 2, a->out); + } +} + +void SPHP::setBuffers_sphp(SPHP *a, float* in, float* out) +{ + a->in = in; + a->out = out; +} + +void SPHP::setSamplerate_sphp(SPHP *a, int rate) +{ + decalc_sphp(a); + a->rate = rate; + calc_sphp(a); +} + +void SPHP::setSize_sphp(SPHP *a, int size) +{ + a->size = size; + flush_sphp(a); +} + +} // namespace WDSP diff --git a/wdsp/sphp.hpp b/wdsp/sphp.hpp new file mode 100644 index 000000000..970e6e92e --- /dev/null +++ b/wdsp/sphp.hpp @@ -0,0 +1,72 @@ +/* sphp.h + +This file is part of a program that implements a Software-Defined Radio. + +Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V +Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel + +This program is free software; you can redistribute it and/or +modify it under the terms of the GNU General Public License +as published by the Free Software Foundation; either version 2 +of the License, or (at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +The author can be reached by email at + +warren@wpratt.com + +*/ + +/******************************************************************************************************** +* * +* Double Single-Pole High-Pass * +* * +********************************************************************************************************/ + +#ifndef wdsp_sphp_h +#define wdsp_sphp_h + +#include "export.h" + +namespace WDSP { + +class WDSP_API SPHP +{ +public: + int run; + int size; + float* in; + float* out; + double rate; + double fc; + int nstages; + double a1, b0, b1; + double* x0, * x1, * y0, * y1; + + // Complex Single-Pole High-Pass + static SPHP* create_sphp(int run, int size, float* in, float* out, double rate, double fc, int nstages); + static void destroy_sphp(SPHP *a); + static void flush_sphp(SPHP *a); + static void xsphp(SPHP *a); + static void setBuffers_sphp(SPHP *a, float* in, float* out); + static void setSamplerate_sphp(SPHP *a, int rate); + static void setSize_sphp(SPHP *a, int size); + +private: + static void calc_sphp(SPHP *a); + static void decalc_sphp(SPHP *a); +}; + +} // namespace WDSP + +#endif + + diff --git a/wdsp/ssql.cpp b/wdsp/ssql.cpp index 0d6283585..b46af706e 100644 --- a/wdsp/ssql.cpp +++ b/wdsp/ssql.cpp @@ -28,7 +28,7 @@ warren@pratt.one #include "comm.hpp" #include "cblock.hpp" #include "ssql.hpp" -#include "iir.hpp" +#include "dbqlp.hpp" #include "RXA.hpp" namespace WDSP { diff --git a/wdsp/ssql.hpp b/wdsp/ssql.hpp index 1927957d1..6b353dc9a 100644 --- a/wdsp/ssql.hpp +++ b/wdsp/ssql.hpp @@ -57,7 +57,7 @@ public: class CBL; class FTDV; -class BQLP; +class DBQLP; class RXA; class WDSP_API SSQL // Syllabic Squelch @@ -85,7 +85,7 @@ public: int* wdbuff; // buffer containing output of window detector CBL *dcbl; // pointer to DC Blocker data structure FTOV *cvtr; // pointer to F to V Converter data structure - BQLP *filt; // pointer to Bi-Quad Low-Pass Filter data structure + DBQLP *filt; // pointer to Bi-Quad Low-Pass Filter data structure int ftov_rsize; // ring size for f_to_v converter double ftov_fmax; // fmax for f_to_v converter // window detector