diff options
author | Andreas Eversberg <jolly@eversberg.eu> | 2020-10-03 16:25:48 +0200 |
---|---|---|
committer | Andreas Eversberg <jolly@eversberg.eu> | 2020-12-29 19:02:57 +0100 |
commit | 58f1c9a91226f4954a4799fab082f186923aa806 (patch) | |
tree | ab137478c73bcb079e3031cbc65ecc7ef37f452e /src/libfilter | |
parent | fde7cc2ce319bf294ded54da0822672fe33b1923 (diff) |
Add libraries from Osmocom-Analog
Diffstat (limited to 'src/libfilter')
-rw-r--r-- | src/libfilter/Makefile.am | 8 | ||||
-rw-r--r-- | src/libfilter/fir_filter.c | 197 | ||||
-rw-r--r-- | src/libfilter/fir_filter.h | 21 | ||||
-rw-r--r-- | src/libfilter/iir_filter.c | 204 | ||||
-rw-r--r-- | src/libfilter/iir_filter.h | 17 |
5 files changed, 447 insertions, 0 deletions
diff --git a/src/libfilter/Makefile.am b/src/libfilter/Makefile.am new file mode 100644 index 0000000..45d2ec2 --- /dev/null +++ b/src/libfilter/Makefile.am @@ -0,0 +1,8 @@ +AM_CPPFLAGS = -Wall -Wextra -g $(all_includes) + +noinst_LIBRARIES = libfilter.a + +libfilter_a_SOURCES = \ + iir_filter.c \ + fir_filter.c + diff --git a/src/libfilter/fir_filter.c b/src/libfilter/fir_filter.c new file mode 100644 index 0000000..5df4f40 --- /dev/null +++ b/src/libfilter/fir_filter.c @@ -0,0 +1,197 @@ +/* FIR filter + * + * (C) 2017 by Andreas Eversberg <jolly@eversberg.eu> + * All Rights Reserved + * + * 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 3 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, see <http://www.gnu.org/licenses/>. + */ + +#include <stdio.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include "../libsample/sample.h" +#include "fir_filter.h" + +//#define DEBUG_TAPS + +static void kernel(double *taps, int M, double cutoff, int invert) +{ + int i; + double sum; + + for (i = 0; i <= M; i++) { + /* gen sinc */ + if (i == M / 2) + taps[i] = 2.0 * M_PI * cutoff; + else + taps[i] = sin(2.0 * M_PI * cutoff * (double)(i - M / 2)) + / (double)(i - M / 2); + /* blackman window */ + taps[i] *= 0.42 - 0.50 * cos(2 * M_PI * (double)(i / M)) + + 0.08 * cos(4 * M_PI * (double)(i / M)); + } + + /* normalize */ + sum = 0; + for (i = 0; i <= M; i++) + sum += taps[i]; + for (i = 0; i <= M; i++) + taps[i] /= sum; + + /* invert */ + if (invert) { + for (i = 0; i <= M; i++) + taps[i] = -taps[i]; + taps[M / 2] += 1.0; + } + +#ifdef DEBUG_TAPS + puts("start"); + for (i = 0; i <= M; i++) + puts(debug_amplitude(taps[i])); +#endif +} + +static fir_filter_t *fir_init(double samplerate, double transition_bandwidth) +{ + fir_filter_t *fir; + int M; + + /* alloc struct */ + fir = calloc(1, sizeof(*fir)); + if (!fir) { + fprintf(stderr, "No memory creating FIR filter!\n"); + return NULL; + } + + /* transition bandwidth */ + M = ceil(1.0 / (transition_bandwidth / samplerate)); + if ((M & 1)) + M++; + +// printf("cutoff=%.4f\n", cutoff / samplerate); +// printf("tb=%.4f\n", transition_bandwidth / samplerate); + fir->ntaps = M + 1; + fir->delay = M / 2; + + /* alloc taps */ + fir->taps = calloc(fir->ntaps, sizeof(*fir->taps)); + if (!fir->taps) { + fprintf(stderr, "No memory creating FIR filter!\n"); + fir_exit(fir); + return NULL; + } + + /* alloc ring buffer */ + fir->buffer = calloc(fir->ntaps, sizeof(*fir->buffer)); + if (!fir->buffer) { + fprintf(stderr, "No memory creating FIR filter!\n"); + fir_exit(fir); + return NULL; + } + + + return fir; +} + +fir_filter_t *fir_lowpass_init(double samplerate, double cutoff, double transition_bandwidth) +{ + /* calculate kernel */ + fir_filter_t *fir = fir_init(samplerate, transition_bandwidth); + if (!fir) + return NULL; + kernel(fir->taps, fir->ntaps - 1, cutoff / samplerate, 0); + return fir; +} + +fir_filter_t *fir_highpass_init(double samplerate, double cutoff, double transition_bandwidth) +{ + fir_filter_t *fir = fir_init(samplerate, transition_bandwidth); + if (!fir) + return NULL; + kernel(fir->taps, fir->ntaps - 1, cutoff / samplerate, 1); + return fir; +} + +fir_filter_t *fir_allpass_init(double samplerate, double transition_bandwidth) +{ + fir_filter_t *fir = fir_init(samplerate, transition_bandwidth); + if (!fir) + return NULL; + fir->taps[(fir->ntaps - 1) / 2] = 1.0; + return fir; +} + +fir_filter_t *fir_twopass_init(double samplerate, double cutoff_low, double cutoff_high, double transition_bandwidth) +{ + int i; + double sum; + fir_filter_t *fir = fir_init(samplerate, transition_bandwidth); + if (!fir) + return NULL; + double lp_taps[fir->ntaps], hp_taps[fir->ntaps]; + kernel(lp_taps, fir->ntaps - 1, cutoff_low / samplerate, 0); + kernel(hp_taps, fir->ntaps - 1, cutoff_high / samplerate, 1); + sum = 0; + printf("#warning does not work as expected\n"); + abort(); + for (i = 0; i < fir->ntaps; i++) { + fir->taps[i] = lp_taps[i] + hp_taps[i]; + sum += fir->taps[i]; + } + /* hp will die */ +// for (i = 0; i < fir->ntaps; i++) + // fir->taps[i] /= sum; + return fir; +} + +void fir_exit(fir_filter_t *fir) +{ + if (!fir) + return; + free(fir->taps); + free(fir->buffer); + free(fir); +} + +void fir_process(fir_filter_t *fir, sample_t *samples, int num) +{ + int i, j; + double y; + + for (i = 0; i < num; i++) { + /* put sample in ring buffer */ + fir->buffer[fir->buffer_pos] = samples[i]; + if (++fir->buffer_pos == fir->ntaps) + fir->buffer_pos = 0; + + /* convolve samples */ + y = 0; + for (j = 0; j < fir->ntaps; j++) { + /* convolve sample from ring buffer, starting with oldes */ + y += fir->buffer[fir->buffer_pos] * fir->taps[j]; + if (++fir->buffer_pos == fir->ntaps) + fir->buffer_pos = 0; + } + samples[i] = y; + } +} + +int fir_get_delay(fir_filter_t *fir) +{ + return fir->delay; +} + diff --git a/src/libfilter/fir_filter.h b/src/libfilter/fir_filter.h new file mode 100644 index 0000000..7d94091 --- /dev/null +++ b/src/libfilter/fir_filter.h @@ -0,0 +1,21 @@ +#ifndef _FIR_FILTER_H +#define _FIR_FILTER_H + +typedef struct fir_filter { + int ntaps; + int delay; + double *taps; + double *buffer; + int buffer_pos; +} fir_filter_t; + +fir_filter_t *fir_lowpass_init(double samplerate, double cutoff, double transition_bandwidth); +fir_filter_t *fir_highpass_init(double samplerate, double cutoff, double transition_bandwidth); +fir_filter_t *fir_allpass_init(double samplerate, double transition_bandwidth); +fir_filter_t *fir_twopass_init(double samplerate, double cutoff_low, double cutoff_high, double transition_bandwidth); +void fir_exit(fir_filter_t *fir); +void fir_process(fir_filter_t *fir, sample_t *samples, int num); +int fir_get_delay(fir_filter_t *fir); + +#endif /* _FIR_FILTER_H */ + diff --git a/src/libfilter/iir_filter.c b/src/libfilter/iir_filter.c new file mode 100644 index 0000000..5872d61 --- /dev/null +++ b/src/libfilter/iir_filter.c @@ -0,0 +1,204 @@ +/* cut-off filter (biquad) based on Nigel Redmon (www.earlevel.com) + * + * (C) 2016 by Andreas Eversberg <jolly@eversberg.eu> + * All Rights Reserved + * + * 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 3 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, see <http://www.gnu.org/licenses/>. + */ + +#include <stdio.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include "../libsample/sample.h" +#include "iir_filter.h" + +//#define DEBUG_NAN + +#define PI M_PI + +void iir_lowpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations) +{ + double Fc, Q, K, norm; + + if (iterations > 64) { + fprintf(stderr, "%s failed: too many iterations, please fix!\n", __func__); + abort(); + } + + memset(filter, 0, sizeof(*filter)); + filter->iter = iterations; + Q = pow(sqrt(0.5), 1.0 / (double)iterations); /* 0.7071 @ 1 iteration */ + Fc = frequency / (double)samplerate; + K = tan(PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = K * K * norm; + filter->a1 = 2 * filter->a0; + filter->a2 = filter->a0; + filter->b1 = 2 * (K * K - 1) * norm; + filter->b2 = (1 - K / Q + K * K) * norm; +#ifdef DEBUG_NAN + printf("%p\n", filter); +#endif +} + +void iir_highpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations) +{ + double Fc, Q, K, norm; + + memset(filter, 0, sizeof(*filter)); + filter->iter = iterations; + Q = pow(sqrt(0.5), 1.0 / (double)iterations); /* 0.7071 @ 1 iteration */ + Fc = frequency / (double)samplerate; + K = tan(PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = 1 * norm; + filter->a1 = -2 * filter->a0; + filter->a2 = filter->a0; + filter->b1 = 2 * (K * K - 1) * norm; + filter->b2 = (1 - K / Q + K * K) * norm; +} + +void iir_bandpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations) +{ + double Fc, Q, K, norm; + + memset(filter, 0, sizeof(*filter)); + filter->iter = iterations; + Q = pow(sqrt(0.5), 1.0 / (double)iterations); /* 0.7071 @ 1 iteration */ + Fc = frequency / (double)samplerate; + K = tan(PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = K / Q * norm; + filter->a1 = 0; + filter->a2 = -filter->a0; + filter->b1 = 2 * (K * K - 1) * norm; + filter->b2 = (1 - K / Q + K * K) * norm; +} + +void iir_notch_init(iir_filter_t *filter, double frequency, int samplerate, int iterations) +{ + double Fc, Q, K, norm; + + memset(filter, 0, sizeof(*filter)); + filter->iter = iterations; + Q = pow(sqrt(0.5), 1.0 / (double)iterations); /* 0.7071 @ 1 iteration */ + Fc = frequency / (double)samplerate; + K = tan(PI * Fc); + norm = 1 / (1 + K / Q + K * K); + filter->a0 = (1 + K * K) * norm; + filter->a1 = 2 * (K * K - 1) * norm; + filter->a2 = filter->a0; + filter->b1 = filter->a1; + filter->b2 = (1 - K / Q + K * K) * norm; +} + +void iir_process(iir_filter_t *filter, sample_t *samples, int length) +{ + double a0, a1, a2, b1, b2; + double *z1, *z2; + double in, out; + int iterations = filter->iter; + int i, j; + + /* get states */ + a0 = filter->a0; + a1 = filter->a1; + a2 = filter->a2; + b1 = filter->b1; + b2 = filter->b2; + + /* these are state pointers, so no need to write back */ + z1 = filter->z1; + z2 = filter->z2; + + /* process filter */ + for (i = 0; i < length; i++) { + /* add a small value, otherwise this loop will perform really bad on my 'nuedel' machine!!! */ + in = *samples + 0.000000001; + for (j = 0; j < iterations; j++) { + out = in * a0 + z1[j]; + z1[j] = in * a1 + z2[j] - b1 * out; + z2[j] = in * a2 - b2 * out; + in = out; + } + *samples++ = in; + } +} + +#ifdef DEBUG_NAN +#pragma GCC push_options +//#pragma GCC optimize ("O0") +#endif + +void iir_process_baseband(iir_filter_t *filter, float *baseband, int length) +{ + double a0, a1, a2, b1, b2; + double *z1, *z2; + double in, out; + int iterations = filter->iter; + int i, j; + + /* get states */ + a0 = filter->a0; + a1 = filter->a1; + a2 = filter->a2; + b1 = filter->b1; + b2 = filter->b2; + + /* these are state pointers, so no need to write back */ + z1 = filter->z1; + z2 = filter->z2; + + /* process filter */ + for (i = 0; i < length; i++) { + /* add a small value, otherwise this loop will perform really bad on my 'nuedel' machine!!! */ + in = *baseband + 0.000000001; + for (j = 0; j < iterations; j++) { + out = in * a0 + z1[j]; +#ifdef DEBUG_NAN + if (!(out > -100 && out < 100)) { + printf("%p\n", filter); + printf("1. i=%d j=%d z=%.5f in=%.5f a0=%.5f out=%.5f\n", i, j, z1[j], in, a0, out); + abort(); + } +#endif + z1[j] = in * a1 + z2[j] - b1 * out; +#ifdef DEBUG_NAN + if (!(z1[j] > -100 && z1[j] < 100)) { + printf("%p\n", filter); + printf("2. i=%d j=%d z1=%.5f z2=%.5f in=%.5f a1=%.5f out=%.5f b1=%.5f\n", i, j, z1[j], z2[j], in, a1, out, b1); + abort(); + } +#endif + z2[j] = in * a2 - b2 * out; +#ifdef DEBUG_NAN + if (!(z2[j] > -100 && z2[j] < 100)) { + printf("%p\n", filter); + printf("%.5f\n", (in * a2) - (b2 * out)); + printf("3. i=%d j=%d z2=%.5f in=%.5f a2=%.5f b2=%.5f out=%.5f\n", i, j, z2[j], in, a2, b2, out); + abort(); + } +#endif + in = out; + } + *baseband = in; + baseband += 2; + } +} + +#ifdef DEBUG_NAN +#pragma GCC pop_options +#endif diff --git a/src/libfilter/iir_filter.h b/src/libfilter/iir_filter.h new file mode 100644 index 0000000..a5956c8 --- /dev/null +++ b/src/libfilter/iir_filter.h @@ -0,0 +1,17 @@ +#ifndef _FILTER_H +#define _FILTER_H + +typedef struct iir_filter { + int iter; + double a0, a1, a2, b1, b2; + double z1[64], z2[64]; +} iir_filter_t; + +void iir_lowpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations); +void iir_highpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations); +void iir_bandpass_init(iir_filter_t *filter, double frequency, int samplerate, int iterations); +void iir_notch_init(iir_filter_t *filter, double frequency, int samplerate, int iterations); +void iir_process(iir_filter_t *filter, sample_t *samples, int length); +void iir_process_baseband(iir_filter_t *filter, float *baseband, int length); + +#endif /* _FILTER_H */ |