diff --git a/.gitignore b/.gitignore index 42ad00d..4c188b3 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,6 @@ config.sub configure depcomp install-sh -libcolorize.pc libtool ltmain.sh missing @@ -28,6 +27,7 @@ src/cnetz/cnetz src/nmt/nmt src/amps/amps sim/cnetz_sim +src/test/test_filter src/test/test_compandor src/test/test_emphasis src/test/test_dms diff --git a/src/cnetz/scrambler.c b/src/cnetz/scrambler.c index 461fe76..2b49335 100644 --- a/src/cnetz/scrambler.c +++ b/src/cnetz/scrambler.c @@ -48,7 +48,7 @@ void scrambler_init(void) void scrambler_setup(scrambler_t *scrambler, int samplerate) { - filter_lowpass_init(&scrambler->lp, CARRIER_HZ - FILTER_BELOW, samplerate); + filter_lowpass_init(&scrambler->lp, CARRIER_HZ - FILTER_BELOW, samplerate, FILTER_TURNS); scrambler->carrier_phaseshift256 = 256.0 / ((double)samplerate / CARRIER_HZ); } @@ -77,7 +77,7 @@ void scrambler(scrambler_t *scrambler, int16_t *samples, int length) scrambler->carrier_phase256 = phase; /* cut off carrier frequency and modulation above carrier frequency */ - filter_lowpass_process(&scrambler->lp, spl, length, FILTER_TURNS); + filter_process(&scrambler->lp, spl, length); for (i = 0; i < length; i++) { /* store result */ diff --git a/src/cnetz/scrambler.h b/src/cnetz/scrambler.h index 757940b..0aea77d 100644 --- a/src/cnetz/scrambler.h +++ b/src/cnetz/scrambler.h @@ -1,9 +1,9 @@ #include "../common/filter.h" typedef struct scrambler { - double carrier_phaseshift256; /* carrier phase shift per sample */ - double carrier_phase256; /* current phase of carrier frequency */ - filter_lowpass_t lp; /* filter to remove carrier frequency */ + double carrier_phaseshift256; /* carrier phase shift per sample */ + double carrier_phase256; /* current phase of carrier frequency */ + filter_t lp; /* filter to remove carrier frequency */ } scrambler_t; void scrambler_init(void); diff --git a/src/common/filter.c b/src/common/filter.c index f568f1b..3aea327 100644 --- a/src/common/filter.c +++ b/src/common/filter.c @@ -26,14 +26,18 @@ #define PI M_PI -//#define CASCADE - -void filter_lowpass_init(filter_lowpass_t *bq, double frequency, int samplerate) +void filter_lowpass_init(filter_t *bq, 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(bq, 0, sizeof(*bq)); - Q = sqrt(0.5); /* 0.7071... */ + bq->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); @@ -44,18 +48,31 @@ void filter_lowpass_init(filter_lowpass_t *bq, double frequency, int samplerate) bq->b2 = (1 - K / Q + K * K) * norm; } -void filter_lowpass_process(filter_lowpass_t *bq, double *samples, int length, int iterations) +void filter_highpass_init(filter_t *bq, double frequency, int samplerate, int iterations) +{ + double Fc, Q, K, norm; + + memset(bq, 0, sizeof(*bq)); + bq->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); + bq->a0 = 1 * norm; + bq->a1 = -2 * bq->a0; + bq->a2 = bq->a0; + bq->b1 = 2 * (K * K - 1) * norm; + bq->b2 = (1 - K / Q + K * K) * norm; +} + +void filter_process(filter_t *bq, double *samples, int length) { double a0, a1, a2, b1, b2; double *z1, *z2; double in, out; + int iterations = bq->iter; int i, j; - if (iterations > 10) { - fprintf(stderr, "%s failed: too many iterations, please fix!\n", __func__); - abort(); - } - /* get states */ a0 = bq->a0; a1 = bq->a1; diff --git a/src/common/filter.h b/src/common/filter.h index 09a379b..42f051b 100644 --- a/src/common/filter.h +++ b/src/common/filter.h @@ -1,12 +1,14 @@ #ifndef _FILTER_H #define _FILTER_H -typedef struct filter_lowpass { +typedef struct filter { + int iter; double a0, a1, a2, b1, b2; - double z1[10], z2[10]; -} filter_lowpass_t; + double z1[64], z2[64]; +} filter_t; -void filter_lowpass_init(filter_lowpass_t *bq, double frequency, int samplerate); -void filter_lowpass_process(filter_lowpass_t *bq, double *samples, int length, int iterations); +void filter_lowpass_init(filter_t *bq, double frequency, int samplerate, int iterations); +void filter_highpass_init(filter_t *bq, double frequency, int samplerate, int iterations); +void filter_process(filter_t *bq, double *samples, int length); #endif /* _FILTER_H */ diff --git a/src/common/samplerate.c b/src/common/samplerate.c index 8113055..f554cf0 100644 --- a/src/common/samplerate.c +++ b/src/common/samplerate.c @@ -37,8 +37,8 @@ int init_samplerate(samplerate_t *state, double samplerate) memset(state, 0, sizeof(*state)); state->factor = samplerate / 8000.0; - filter_lowpass_init(&state->up.lp, 4000.0, samplerate); - filter_lowpass_init(&state->down.lp, 4000.0, samplerate); + filter_lowpass_init(&state->up.lp, 4000.0, samplerate, 1); + filter_lowpass_init(&state->down.lp, 4000.0, samplerate, 1); return 0; } @@ -56,7 +56,7 @@ int samplerate_downsample(samplerate_t *state, int16_t *input, int input_num, in spl[i] = *input++ / 32768.0; /* filter down */ - filter_lowpass_process(&state->down.lp, spl, input_num, 1); + filter_process(&state->down.lp, spl, input_num); /* resample filtered result */ in_index = state->down.in_index; @@ -125,7 +125,7 @@ int samplerate_upsample(samplerate_t *state, int16_t *input, int input_num, int1 state->up.in_index = in_index; /* filter up */ - filter_lowpass_process(&state->up.lp, spl, output_num, 1); + filter_process(&state->up.lp, spl, output_num); /* convert double to samples */ for (i = 0; i < output_num; i++) { diff --git a/src/common/samplerate.h b/src/common/samplerate.h index 27955de..d8c9d41 100644 --- a/src/common/samplerate.h +++ b/src/common/samplerate.h @@ -3,11 +3,11 @@ typedef struct samplerate { double factor; struct { - filter_lowpass_t lp; + filter_t lp; double in_index; } down; struct { - filter_lowpass_t lp; + filter_t lp; double in_index; } up; } samplerate_t; diff --git a/src/common/sdr.c b/src/common/sdr.c index 2644968..db46906 100644 --- a/src/common/sdr.c +++ b/src/common/sdr.c @@ -39,7 +39,7 @@ typedef struct sdr_chan { double rx_rot; /* rotation step per sample to shift rx frequency (used to shift) */ double rx_phase; /* current rotation phase (used to shift) */ double rx_last_phase; /* last phase of FM (used to demodulate) */ - filter_lowpass_t rx_lp[2]; /* filters received IQ signal */ + filter_t rx_lp[2]; /* filters received IQ signal */ } sdr_chan_t; typedef struct sdr { @@ -123,8 +123,8 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq PDEBUG(DSDR, DEBUG_INFO, "Frequency #%d: TX = %.6f MHz, RX = %.6f MHz\n", c, tx_frequency[c] / 1e6, rx_frequency[c] / 1e6); sdr->chan[c].tx_frequency = tx_frequency[c]; sdr->chan[c].rx_frequency = rx_frequency[c]; - filter_lowpass_init(&sdr->chan[c].rx_lp[0], bandwidth, samplerate); - filter_lowpass_init(&sdr->chan[c].rx_lp[1], bandwidth, samplerate); + filter_lowpass_init(&sdr->chan[c].rx_lp[0], bandwidth, samplerate, 1); + filter_lowpass_init(&sdr->chan[c].rx_lp[1], bandwidth, samplerate, 1); } if (sdr->paging_channel) { PDEBUG(DSDR, DEBUG_INFO, "Paging Frequency: TX = %.6f MHz\n", paging_frequency / 1e6); @@ -378,8 +378,8 @@ int sdr_read(void *inst, int16_t **samples, int num, int channels) Q[s] = i * sin(phase) + q * cos(phase); } sdr->chan[c].rx_phase = phase; - filter_lowpass_process(&sdr->chan[c].rx_lp[0], I, count, 1); - filter_lowpass_process(&sdr->chan[c].rx_lp[1], Q, count, 1); + filter_process(&sdr->chan[c].rx_lp[0], I, count); + filter_process(&sdr->chan[c].rx_lp[1], Q, count); last_phase = sdr->chan[c].rx_last_phase; for (s = 0; s < count; s++) { phase = atan2(Q[s], I[s]); diff --git a/src/test/Makefile.am b/src/test/Makefile.am index 25211d2..eace0dc 100644 --- a/src/test/Makefile.am +++ b/src/test/Makefile.am @@ -2,11 +2,21 @@ AUTOMAKE_OPTIONS = subdir-objects AM_CPPFLAGS = -Wall -g $(all_includes) noinst_PROGRAMS = \ + test_filter \ test_compandor \ test_emphasis \ test_dms \ test_sms +test_filter_SOURCES = test_filter.c dummy.c + +test_filter_LDADD = \ + $(COMMON_LA) \ + $(top_builddir)/src/common/libcommon.a \ + $(ALSA_LIBS) \ + $(UHD_LIBS) \ + -lm + test_compandor_SOURCES = test_compandor.c test_compandor_LDADD = \ diff --git a/src/test/test_filter.c b/src/test/test_filter.c new file mode 100644 index 0000000..8c82292 --- /dev/null +++ b/src/test/test_filter.c @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include "../common/filter.h" +#include "../common/debug.h" + +#define level2db(level) (20 * log10(level)) +#define db2level(db) pow(10, (double)db / 20.0) + +#define SAMPLERATE 48000 + +static double get_level(double *samples) +{ +#if 0 + int i; + double last = 0, envelope = 0; + int up = 0; + + for (i = SAMPLERATE/2; i < SAMPLERATE; i++) { + if (last < samples[i]) { + up = 1; + } else if (last > samples[i]) { + if (up) { + if (last > envelope) + envelope = last; + } + up = 0; + } + last = samples[i]; + } +#else + int i; + double envelope = 0; + for (i = SAMPLERATE/2; i < SAMPLERATE; i++) { + if (samples[i] > envelope) + envelope = samples[i]; + } +#endif + + return envelope; +} + +static void gen_samples(double *samples, double freq) +{ + int i; + double value; + + for (i = 0; i < SAMPLERATE; i++) { + value = cos(2.0 * M_PI * freq / (double)SAMPLERATE * (double)i); + samples[i] = value; + } +} + +int main(void) +{ + filter_t filter_low; + filter_t filter_high; + double samples[SAMPLERATE]; + double level; + int iter = 2; + int i; + + debuglevel = DEBUG_DEBUG; + + printf("testing low-pass filter with %d iterations\n", iter); + + filter_lowpass_init(&filter_low, 1000.0, SAMPLERATE, iter); + + for (i = 0; i < 4001; i += 100) { + gen_samples(samples, (double)i); + filter_process(&filter_low, samples, SAMPLERATE); + level = get_level(samples); + printf("%4d Hz: %.1f dB", i, level2db(level)); + if (i == 1000) + printf(" cutoff\n"); + else if (i == 2000) + printf(" double frequency\n"); + else if (i == 4000) + printf(" quad frequency\n"); + else + printf("\n"); + } + + printf("testing high-pass filter with %d iterations\n", iter); + + filter_highpass_init(&filter_high, 2000.0, SAMPLERATE, iter); + + for (i = 0; i < 4001; i += 100) { + gen_samples(samples, (double)i); + filter_process(&filter_high, samples, SAMPLERATE); + level = get_level(samples); + printf("%4d Hz: %.1f dB", i, level2db(level)); + if (i == 2000) + printf(" cutoff\n"); + else if (i == 1000) + printf(" half frequency\n"); + else if (i == 500) + printf(" quarter frequency\n"); + else + printf("\n"); + } + + printf("testing band-pass filter with %d iterations\n", iter); + + filter_lowpass_init(&filter_low, 2000.0, SAMPLERATE, iter); + filter_highpass_init(&filter_high, 1000.0, SAMPLERATE, iter); + + for (i = 0; i < 4001; i += 100) { + gen_samples(samples, (double)i); + filter_process(&filter_low, samples, SAMPLERATE); + filter_process(&filter_high, samples, SAMPLERATE); + level = get_level(samples); + printf("%4d Hz: %.1f dB", i, level2db(level)); + if (i == 1000) + printf(" cutoff high\n"); + else if (i == 2000) + printf(" cutoff low\n"); + else + printf("\n"); + } + + return 0; +} +