From 290b365d679384fdc864f442b1a03245faea1b39 Mon Sep 17 00:00:00 2001 From: Andreas Eversberg Date: Thu, 9 Feb 2017 19:24:09 +0100 Subject: SDR: Move FM modulation algorithms to a seperate file --- src/common/Makefile.am | 1 + src/common/fm_modulation.c | 188 +++++++++++++++++++++++++++++++++++++++++++++ src/common/fm_modulation.h | 24 ++++++ src/common/sdr.c | 125 +++++++----------------------- 4 files changed, 240 insertions(+), 98 deletions(-) create mode 100644 src/common/fm_modulation.c create mode 100644 src/common/fm_modulation.h (limited to 'src/common') diff --git a/src/common/Makefile.am b/src/common/Makefile.am index c19c71b..4ac6c67 100644 --- a/src/common/Makefile.am +++ b/src/common/Makefile.am @@ -24,6 +24,7 @@ libcommon_a_SOURCES = \ ../common/emphasis.c \ ../common/compandor.c \ ../common/fft.c \ + ../common/fm_modulation.c \ ../common/sender.c \ ../common/display_wave.c \ ../common/main_common.c diff --git a/src/common/fm_modulation.c b/src/common/fm_modulation.c new file mode 100644 index 0000000..dca88b0 --- /dev/null +++ b/src/common/fm_modulation.c @@ -0,0 +1,188 @@ +/* FM modulation processing + * + * (C) 2017 by Andreas Eversberg + * 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 . + */ + +#include +#include +#include +#include +#include +#include "sample.h" +#include "filter.h" +#include "fm_modulation.h" + +//#define FAST_SINE + +/* init FM modulator */ +void fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude) +{ + memset(mod, 0, sizeof(*mod)); + mod->samplerate = samplerate; + mod->offset = offset; + mod->amplitude = amplitude; + +#ifdef FAST_SINE + int i; + + mod->sin_tab = calloc(65536+16384, sizeof(*mod->sin_tab)); + if (!mod->sin_tab) { + fprintf(stderr, "No mem!\n"); + abort(); + } + + /* generate sine and cosine */ + for (i = 0; i < 65536+16384; i++) + mod->sin_tab[i] = sin(2.0 * M_PI * (double)i / 65536.0) * amplitude; +#endif +} + +/* do frequency modulation of samples and add them to existing buff */ +void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff) +{ + double dev, rate, phase, offset; + int s, ss; +#ifdef FAST_SINE + double *sin_tab, *cos_tab; +#else + double amplitude; +#endif + + rate = mod->samplerate; + phase = mod->phase; + offset = mod->offset; +#ifdef FAST_SINE + sin_tab = mod->sin_tab; + cos_tab = mod->sin_tab + 16384; +#else + amplitude = mod->amplitude; +#endif + + /* modulate */ + for (s = 0, ss = 0; s < num; s++) { + /* deviation is defined by the sample value and the offset */ + dev = offset + samples[s]; +#ifdef FAST_SINE + phase += 65536.0 * dev / rate; + if (phase < 0.0) + phase += 65536.0; + else if (phase >= 65536.0) + phase -= 65536.0; + buff[ss++] += cos_tab[(uint16_t)phase]; + buff[ss++] += sin_tab[(uint16_t)phase]; +#else + phase += 2.0 * M_PI * dev / rate; + if (phase < 0.0) + phase += 2.0 * M_PI; + else if (phase >= 2.0 * M_PI) + phase -= 2.0 * M_PI; + buff[ss++] += cos(phase) * amplitude; + buff[ss++] += sin(phase) * amplitude; +#endif + } + + mod->phase = phase; +} + +/* init FM demodulator */ +void fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth) +{ + memset(demod, 0, sizeof(*demod)); + demod->samplerate = samplerate; +#ifdef FAST_SINE + demod->rot = 65536.0 * -offset / samplerate; +#else + demod->rot = 2 * M_PI * -offset / samplerate; +#endif + + /* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */ + filter_lowpass_init(&demod->lp[0], bandwidth / 2.0, samplerate, 2); + filter_lowpass_init(&demod->lp[1], bandwidth / 2.0, samplerate, 2); + +#ifdef FAST_SINE + int i; + + demod->sin_tab = calloc(65536+16384, sizeof(*demod->sin_tab)); + if (!demod->sin_tab) { + fprintf(stderr, "No mem!\n"); + abort(); + } + + /* generate sine and cosine */ + for (i = 0; i < 65536+16384; i++) + demod->sin_tab[i] = sin(2.0 * M_PI * (double)i / 65536.0); +#endif +} + +/* do frequency demodulation of buff and write them to samples */ +void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff) +{ + double phase, rot, last_phase, dev, rate; + double _sin, _cos; + sample_t I[num], Q[num], i, q; + int s, ss; +#ifdef FAST_SINE + double *sin_tab, *cos_tab; +#endif + + rate = demod->samplerate; + phase = demod->phase; + rot = demod->rot; +#ifdef FAST_SINE + sin_tab = demod->sin_tab; + cos_tab = demod->sin_tab + 16384; +#endif + for (s = 0, ss = 0; s < num; s++) { + phase += rot; + i = buff[ss++]; + q = buff[ss++]; +#ifdef FAST_SINE + if (phase < 0.0) + phase += 65536.0; + else if (phase >= 65536.0) + phase -= 65536.0; + _sin = sin_tab[(uint16_t)phase]; + _cos = cos_tab[(uint16_t)phase]; +#else + if (phase < 0.0) + phase += 2.0 * M_PI; + else if (phase >= 2.0 * M_PI) + phase -= 2.0 * M_PI; + _sin = sin(phase); + _cos = cos(phase); +#endif + I[s] = i * _cos - q * _sin; + Q[s] = i * _sin + q * _cos; + } + demod->phase = phase; + filter_process(&demod->lp[0], I, num); + filter_process(&demod->lp[1], Q, num); + last_phase = demod->last_phase; + for (s = 0; s < num; s++) { + phase = atan2(Q[s], I[s]); + dev = (phase - last_phase) / 2 / M_PI; + last_phase = phase; + if (dev < -0.49) + dev += 1.0; + else if (dev > 0.49) + dev -= 1.0; + dev *= rate; + samples[s] = dev; + } + demod->last_phase = last_phase; +} + diff --git a/src/common/fm_modulation.h b/src/common/fm_modulation.h new file mode 100644 index 0000000..194ae3f --- /dev/null +++ b/src/common/fm_modulation.h @@ -0,0 +1,24 @@ + +typedef struct fm_mod { + double samplerate; /* sample rate of in and out */ + double offset; /* offset to calculated center frequency */ + double amplitude; /* how much amplitude to add to the buff */ + double phase; /* current phase of FM (used to shift and modulate ) */ + double *sin_tab; /* sine/cosine table for modulation */ +} fm_mod_t; + +void fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude); +void fm_modulate(fm_mod_t *mod, sample_t *samples, int num, float *buff); + +typedef struct fm_demod { + double samplerate; /* sample rate of in and out */ + double phase; /* current rotation phase (used to shift) */ + double rot; /* rotation step per sample to shift rx frequency (used to shift) */ + double last_phase; /* last phase of FM (used to demodulate) */ + filter_t lp[2]; /* filters received IQ signal */ + double *sin_tab; /* sine/cosine table rotation */ +} fm_demod_t; + +void fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth); +void fm_demodulate(fm_demod_t *demod, sample_t *samples, int num, float *buff); + diff --git a/src/common/sdr.c b/src/common/sdr.c index 60afbca..0d0a595 100644 --- a/src/common/sdr.c +++ b/src/common/sdr.c @@ -24,55 +24,38 @@ #include #include "sample.h" #include "filter.h" +#include "fm_modulation.h" #include "sender.h" #ifdef HAVE_UHD #include "uhd.h" #endif #include "debug.h" -//#define FAST_SINE - typedef struct sdr_chan { - double tx_frequency; /* frequency used */ - double rx_frequency; /* frequency used */ - double offset; /* offset to calculated center frequency */ - double tx_phase; /* current phase of FM (used to shift and modulate ) */ - 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_t rx_lp[2]; /* filters received IQ signal */ + double tx_frequency; /* frequency used */ + double rx_frequency; /* frequency used */ + fm_mod_t mod; /* modulator instance */ + fm_demod_t demod; /* demodulator instance */ } sdr_chan_t; typedef struct sdr { - sdr_chan_t *chan; /* settings for all channels */ - int paging_channel; /* if set, points to paging channel */ - sdr_chan_t paging_chan; /* settings for extra paging channel */ - int channels; /* number of frequencies */ - double samplerate; /* IQ rate */ - double amplitude; /* amplitude of each carrier */ - wave_rec_t wave_rx_rec; - wave_rec_t wave_tx_rec; - wave_play_t wave_rx_play; + sdr_chan_t *chan; /* settings for all channels */ + int paging_channel; /* if set, points to paging channel */ + sdr_chan_t paging_chan; /* settings for extra paging channel */ + int channels; /* number of frequencies */ + double samplerate; /* IQ rate */ + double amplitude; /* amplitude of each carrier */ + wave_rec_t wave_rx_rec; + wave_rec_t wave_tx_rec; + wave_play_t wave_rx_play; } sdr_t; static const char *sdr_device_args; static double sdr_rx_gain, sdr_tx_gain; const char *sdr_write_iq_rx_wave, *sdr_write_iq_tx_wave, *sdr_read_iq_rx_wave; -#ifdef FAST_SINE -static float sdr_sine[256]; -#endif - int sdr_init(const char *device_args, double rx_gain, double tx_gain, const char *write_iq_rx_wave, const char *write_iq_tx_wave, const char *read_iq_rx_wave) { -#ifdef FAST_SINE - int i; - - for (i = 0; i < 256; i++) { - sdr_sine[i] = sin(2.0*M_PI*i/256); - } -#endif - sdr_device_args = strdup(device_args); sdr_rx_gain = rx_gain; sdr_tx_gain = tx_gain; @@ -127,8 +110,6 @@ 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 / 2.0, samplerate, 1); - filter_lowpass_init(&sdr->chan[c].rx_lp[1], bandwidth / 2.0, samplerate, 1); } if (sdr->paging_channel) { PDEBUG(DSDR, DEBUG_INFO, "Paging Frequency: TX = %.6f MHz\n", paging_frequency / 1e6); @@ -180,15 +161,18 @@ void *sdr_open(const char __attribute__((__unused__)) *audiodev, double *tx_freq PDEBUG(DSDR, DEBUG_INFO, "Using center frequency: TX %.6f MHz, RX %.6f\n", tx_center_frequency / 1e6, rx_center_frequency / 1e6); /* set offsets to center frequency */ for (c = 0; c < channels; c++) { - double rx_offset; - sdr->chan[c].offset = sdr->chan[c].tx_frequency - tx_center_frequency; + double tx_offset, rx_offset; + tx_offset = sdr->chan[c].tx_frequency - tx_center_frequency; rx_offset = sdr->chan[c].rx_frequency - rx_center_frequency; - sdr->chan[c].rx_rot = 2 * M_PI * -rx_offset / sdr->samplerate; - PDEBUG(DSDR, DEBUG_DEBUG, "Frequency #%d: TX offset: %.6f MHz, RX offset: %.6f MHz\n", c, sdr->chan[c].offset / 1e6, rx_offset / 1e6); + PDEBUG(DSDR, DEBUG_DEBUG, "Frequency #%d: TX offset: %.6f MHz, RX offset: %.6f MHz\n", c, tx_offset / 1e6, rx_offset / 1e6); + fm_mod_init(&sdr->chan[c].mod, sdr->samplerate, tx_offset, sdr->amplitude); + fm_demod_init(&sdr->chan[c].demod, sdr->samplerate, rx_offset, bandwidth); } if (sdr->paging_channel) { - sdr->chan[sdr->paging_channel].offset = sdr->chan[sdr->paging_channel].tx_frequency - tx_center_frequency; - PDEBUG(DSDR, DEBUG_DEBUG, "Paging Frequency: TX offset: %.6f MHz\n", sdr->chan[sdr->paging_channel].offset / 1e6); + double tx_offset; + tx_offset = sdr->chan[sdr->paging_channel].tx_frequency - tx_center_frequency; + PDEBUG(DSDR, DEBUG_DEBUG, "Paging Frequency: TX offset: %.6f MHz\n", tx_offset / 1e6); + fm_mod_init(&sdr->chan[sdr->paging_channel].mod, sdr->samplerate, tx_offset, sdr->amplitude); } PDEBUG(DSDR, DEBUG_INFO, "Using gain: TX %.1f dB, RX %.1f dB\n", sdr_tx_gain, sdr_rx_gain); @@ -250,7 +234,6 @@ int sdr_write(void *inst, sample_t **samples, int num, enum paging_signal __attr sdr_t *sdr = (sdr_t *)inst; float buff[num * 2]; int c, s, ss; - double rate, offset, phase, amplitude, dev; int sent; if (channels != sdr->channels) { @@ -259,39 +242,13 @@ int sdr_write(void *inst, sample_t **samples, int num, enum paging_signal __attr } /* process all channels */ - rate = sdr->samplerate; - amplitude = sdr->amplitude; memset(buff, 0, sizeof(buff)); for (c = 0; c < channels; c++) { - phase = sdr->chan[c].tx_phase; - /* switch offset to paging channel, if requested */ + /* switch to paging channel, if requested */ if (on[c] && sdr->paging_channel) - offset = sdr->chan[sdr->paging_channel].offset; + fm_modulate(&sdr->chan[sdr->paging_channel].mod, samples[c], num, buff); else - offset = sdr->chan[c].offset; - /* modulate */ - for (s = 0, ss = 0; s < num; s++) { - /* deviation is defined by the sample value and the offset */ - dev = offset + samples[c][s]; -#ifdef FAST_SINE - phase += 256.0 * dev / rate; - if (phase < 0.0) - phase += 256.0; - if (phase >= 256.0) - phase -= 256.0; - buff[ss++] += sdr_sine[((int)phase + 64) & 0xff] * amplitude; - buff[ss++] += sdr_sine[(int)phase & 0xff] * amplitude; -#else - phase += 2.0 * M_PI * dev / rate; - if (phase < 0.0) - phase += 2.0 * M_PI; - if (phase >= 2.0 * M_PI) - phase -= 2.0 * M_PI; - buff[ss++] += cos(phase) * amplitude; - buff[ss++] += sin(phase) * amplitude; -#endif - } - sdr->chan[c].tx_phase = phase; + fm_modulate(&sdr->chan[c].mod, samples[c], num, buff); } if (sdr->wave_tx_rec.fp) { @@ -316,12 +273,8 @@ int sdr_read(void *inst, sample_t **samples, int num, int channels) { sdr_t *sdr = (sdr_t *)inst; float buff[num * 2]; - sample_t I[num], Q[num], i, q; int count; int c, s, ss; - double phase, rot, last_phase, dev, rate; - - rate = sdr->samplerate; #ifdef HAVE_UHD count = uhd_receive(buff, num); @@ -349,31 +302,7 @@ int sdr_read(void *inst, sample_t **samples, int num, int channels) display_spectrum(buff, count); for (c = 0; c < channels; c++) { - rot = sdr->chan[c].rx_rot; - phase = sdr->chan[c].rx_phase; - for (s = 0, ss = 0; s < count; s++) { - phase += rot; - i = buff[ss++]; - q = buff[ss++]; - I[s] = i * cos(phase) - q * sin(phase); - Q[s] = i * sin(phase) + q * cos(phase); - } - sdr->chan[c].rx_phase = phase; - 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]); - dev = (phase - last_phase) / 2 / M_PI; - last_phase = phase; - if (dev < -0.49) - dev += 1.0; - else if (dev > 0.49) - dev -= 1.0; - dev *= rate; - samples[c][s] = dev; - } - sdr->chan[c].rx_last_phase = last_phase; + fm_demodulate(&sdr->chan[c].demod, samples[c], count, buff); } return count; -- cgit v1.2.3