diff options
Diffstat (limited to 'src/ss5/mf.c')
| -rw-r--r-- | src/ss5/mf.c | 202 |
1 files changed, 202 insertions, 0 deletions
diff --git a/src/ss5/mf.c b/src/ss5/mf.c new file mode 100644 index 0000000..273146e --- /dev/null +++ b/src/ss5/mf.c @@ -0,0 +1,202 @@ +/* multi frequency coder and decoder + * + * (C) 2020 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 <stdlib.h> +#include <stdint.h> +#include <string.h> +#include <errno.h> +#include <math.h> +#include "../libsample/sample.h" +#include "mf.h" + +static int has_init = 0; +static int fast_math = 0; +static float *sin_tab = NULL, *cos_tab = NULL; + +/* global init */ +int mf_init(int _fast_math) +{ + fast_math = _fast_math; + + if (fast_math) { + int i; + + sin_tab = calloc(65536+16384, sizeof(*sin_tab)); + if (!sin_tab) { + fprintf(stderr, "No mem!\n"); + return -ENOMEM; + } + cos_tab = sin_tab + 16384; + + /* generate sine and cosine */ + for (i = 0; i < 65536+16384; i++) + sin_tab[i] = sin(2.0 * M_PI * (double)i / 65536.0); + } + + has_init = 1; + + return 0; +} + +/* global exit */ +void mf_exit(void) +{ + if (sin_tab) { + free(sin_tab); + sin_tab = cos_tab = NULL; + } + + has_init = 0; +} + +mf_mod_t *mf_mod_init(double samplerate, int tones, double *freq, double *amplitude) +{ + mf_mod_t *mod; + int t; + + if (!has_init) { + fprintf(stderr, "libmf was not initialized, plese fix!\n"); + abort(); + } + + mod = calloc(1, sizeof(*mod) + sizeof(*mod->tone) * tones); + if (!mod) { + fprintf(stderr, "No mem!\n"); + return NULL; + } + mod->tones = tones; + + for (t = 0; t < tones; t++) { + /* set rotation of IQ vector */ + if (fast_math) + mod->tone[t].rot = 65536.0 * freq[t] / samplerate; + else + mod->tone[t].rot = 2 * M_PI * freq[t] / samplerate; + mod->tone[t].amplitude = amplitude[t]; + } + + return mod; +} + +void mf_mod_exit(mf_mod_t *mod) +{ + free(mod); +} + +void mf_mod(mf_mod_t *mod, uint32_t *mask, sample_t *samples, int length) +{ + int t, s; + double phase; + + for (s = 0; s < length; s++) { + samples[s] = 0; + for (t = 0; t < mod->tones; t++) { + /* continue phase even on muted tones */ + phase = (mod->tone[t].phase += mod->tone[t].rot); + if (!((1 << t) & mask[s])) + continue; + if (fast_math) { + if (phase >= 65536.0) + phase -= 65536.0; + samples[s] += sin_tab[(uint16_t)phase] * mod->tone[t].amplitude; + } else { + if (phase >= 2.0 * M_PI) + phase -= 2.0 * M_PI; + samples[s] += cos(phase) * mod->tone[t].amplitude; + } + } + } +} + +mf_demod_t *mf_demod_init(double samplerate, int tones, double *freq, double *width) +{ + mf_demod_t *demod; + int t; + + if (!has_init) { + fprintf(stderr, "libmf was not initialized, plese fix!\n"); + abort(); + } + + demod = calloc(1, sizeof(*demod) + sizeof(*demod->tone) * tones); + if (!demod) { + fprintf(stderr, "No mem!\n"); + return NULL; + } + demod->tones = tones; + + for (t = 0; t < tones; t++) { + /* set rotation of IQ vector */ + if (fast_math) + demod->tone[t].rot = 65536.0 * -freq[t] / samplerate; + else + demod->tone[t].rot = 2 * M_PI * -freq[t] / samplerate; + + /* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */ + iir_lowpass_init(&demod->tone[t].lp[0], width[t], samplerate, 2); + iir_lowpass_init(&demod->tone[t].lp[1], width[t], samplerate, 2); + } + + return demod; +} + +void mf_demod_exit(mf_demod_t *demod) +{ + free(demod); +} + +void mf_demod(mf_demod_t *demod, sample_t *samples, int length, sample_t **levels_squared) +{ + sample_t I[length], Q[length]; + double phase, rot; + int t, s; + double i, _sin, _cos; + sample_t *level_squared; + + /* we apply filters and get the squared levels as a result */ + for (t = 0; t < demod->tones; t++) { + phase = demod->tone[t].phase; + rot = demod->tone[t].rot; + for (s = 0; s < length; s++) { + phase += rot; + i = samples[s]; + if (fast_math) { + if (phase >= 65536.0) + phase -= 65536.0; + _sin = sin_tab[(uint16_t)phase]; + _cos = cos_tab[(uint16_t)phase]; + } else { + if (phase >= 2.0 * M_PI) + phase -= 2.0 * M_PI; + _sin = sin(phase); + _cos = cos(phase); + } + I[s] = i * _cos; + Q[s] = i * _sin; + } + demod->tone[t].phase = phase; + iir_process(&demod->tone[t].lp[0], I, length); /* filter single side band */ + iir_process(&demod->tone[t].lp[1], Q, length); + level_squared = levels_squared[t]; + for (s = 0; s < length; s++) + level_squared[s] = 4.0 * (I[s] * I[s] + Q[s] * Q[s]); /* x2 because of single side band */ + } +} + |