Restructure: Move fm_modulation from common code to 'libfm'
This commit is contained in:
Andreas Eversberg
6
src/libfm/Makefile.am
Normal file
6
src/libfm/Makefile.am
Normal file
@@ -0,0 +1,6 @@
|
||||
AM_CPPFLAGS = -Wall -Wextra -g $(all_includes)
|
||||
|
||||
noinst_LIBRARIES = libfm.a
|
||||
|
||||
libfm_a_SOURCES = \
|
||||
fm.c
|
||||
382
src/libfm/fm.c
Normal file
382
src/libfm/fm.c
Normal file
@@ -0,0 +1,382 @@
|
||||
/* FM modulation processing
|
||||
*
|
||||
* (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 <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include "../common/sample.h"
|
||||
#include "fm.h"
|
||||
|
||||
//#define FAST_SINE
|
||||
|
||||
/* init FM modulator */
|
||||
int fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude)
|
||||
{
|
||||
int i;
|
||||
|
||||
memset(mod, 0, sizeof(*mod));
|
||||
mod->samplerate = samplerate;
|
||||
mod->offset = offset;
|
||||
mod->amplitude = amplitude;
|
||||
|
||||
mod->ramp_length = samplerate * 0.001;
|
||||
mod->ramp_tab = calloc(mod->ramp_length, sizeof(*mod->ramp_tab));
|
||||
if (!mod->ramp_tab) {
|
||||
fprintf(stderr, "No mem!\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
mod->state = MOD_STATE_OFF;
|
||||
|
||||
/* generate ramp up with ramp_length */
|
||||
for (i = 0; i < mod->ramp_length; i++)
|
||||
mod->ramp_tab[i] = 0.5 - cos(M_PI * i / mod->ramp_length) / 2.0;
|
||||
|
||||
#ifdef FAST_SINE
|
||||
mod->sin_tab = calloc(65536+16384, sizeof(*mod->sin_tab));
|
||||
if (!mod->sin_tab) {
|
||||
fprintf(stderr, "No mem!\n");
|
||||
fm_mod_exit(mod);
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* 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
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void fm_mod_exit(fm_mod_t *mod)
|
||||
{
|
||||
if (mod->ramp_tab) {
|
||||
free(mod->ramp_tab);
|
||||
mod->ramp_tab = NULL;
|
||||
}
|
||||
if (mod->sin_tab) {
|
||||
free(mod->sin_tab);
|
||||
mod->sin_tab = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/* do frequency modulation of samples and add them to existing baseband */
|
||||
void fm_modulate_complex(fm_mod_t *mod, sample_t *frequency, uint8_t *power, int length, float *baseband)
|
||||
{
|
||||
double dev, rate, phase, offset;
|
||||
int ramp, ramp_length;
|
||||
double *ramp_tab;
|
||||
#ifdef FAST_SINE
|
||||
double *sin_tab, *cos_tab;
|
||||
#else
|
||||
double amplitude;
|
||||
#endif
|
||||
|
||||
rate = mod->samplerate;
|
||||
phase = mod->phase;
|
||||
offset = mod->offset;
|
||||
ramp = mod->ramp;
|
||||
ramp_length = mod->ramp_length;
|
||||
ramp_tab = mod->ramp_tab;
|
||||
#ifdef FAST_SINE
|
||||
sin_tab = mod->sin_tab;
|
||||
cos_tab = mod->sin_tab + 16384;
|
||||
#else
|
||||
amplitude = mod->amplitude;
|
||||
#endif
|
||||
|
||||
again:
|
||||
switch (mod->state) {
|
||||
case MOD_STATE_ON:
|
||||
/* modulate */
|
||||
while (length) {
|
||||
/* is power is not set, ramp down */
|
||||
if (!(*power)) {
|
||||
mod->state = MOD_STATE_RAMP_DOWN;
|
||||
break;
|
||||
}
|
||||
/* deviation is defined by the frequency value and the offset */
|
||||
dev = offset + *frequency++;
|
||||
power++;
|
||||
length--;
|
||||
#ifdef FAST_SINE
|
||||
phase += 65536.0 * dev / rate;
|
||||
if (phase < 0.0)
|
||||
phase += 65536.0;
|
||||
else if (phase >= 65536.0)
|
||||
phase -= 65536.0;
|
||||
*baseband++ += cos_tab[(uint16_t)phase];
|
||||
*baseband++ += 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;
|
||||
*baseband++ += cos(phase) * amplitude;
|
||||
*baseband++ += sin(phase) * amplitude;
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
case MOD_STATE_RAMP_DOWN:
|
||||
while (length) {
|
||||
/* if power is set, ramp up */
|
||||
if (*power) {
|
||||
mod->state = MOD_STATE_RAMP_UP;
|
||||
break;
|
||||
}
|
||||
if (ramp == 0) {
|
||||
mod->state = MOD_STATE_OFF;
|
||||
break;
|
||||
}
|
||||
dev = offset + *frequency++;
|
||||
power++;
|
||||
length--;
|
||||
#ifdef FAST_SINE
|
||||
phase += 65536.0 * dev / rate;
|
||||
if (phase < 0.0)
|
||||
phase += 65536.0;
|
||||
else if (phase >= 65536.0)
|
||||
phase -= 65536.0;
|
||||
*baseband++ += cos_tab[(uint16_t)phase] * ramp_tab[ramp];
|
||||
*baseband++ += sin_tab[(uint16_t)phase] * ramp_tab[ramp];
|
||||
#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;
|
||||
*baseband++ += cos(phase) * amplitude * ramp_tab[ramp];
|
||||
*baseband++ += sin(phase) * amplitude * ramp_tab[ramp];
|
||||
#endif
|
||||
ramp--;
|
||||
}
|
||||
break;
|
||||
case MOD_STATE_OFF:
|
||||
while (length) {
|
||||
/* if power is set, ramp up */
|
||||
if (*power) {
|
||||
mod->state = MOD_STATE_RAMP_UP;
|
||||
break;
|
||||
}
|
||||
frequency++;
|
||||
power++;
|
||||
length--;
|
||||
baseband += 2;
|
||||
}
|
||||
break;
|
||||
case MOD_STATE_RAMP_UP:
|
||||
while (length) {
|
||||
/* is power is not set, ramp down */
|
||||
if (!(*power)) {
|
||||
mod->state = MOD_STATE_RAMP_DOWN;
|
||||
break;
|
||||
}
|
||||
if (ramp == ramp_length - 1) {
|
||||
mod->state = MOD_STATE_ON;
|
||||
break;
|
||||
}
|
||||
/* deviation is defined by the frequency value and the offset */
|
||||
dev = offset + *frequency++;
|
||||
power++;
|
||||
length--;
|
||||
#ifdef FAST_SINE
|
||||
phase += 65536.0 * dev / rate;
|
||||
if (phase < 0.0)
|
||||
phase += 65536.0;
|
||||
else if (phase >= 65536.0)
|
||||
phase -= 65536.0;
|
||||
*baseband++ += cos_tab[(uint16_t)phase] * ramp_tab[ramp];
|
||||
*baseband++ += sin_tab[(uint16_t)phase] * ramp_tab[ramp];
|
||||
#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;
|
||||
*baseband++ += cos(phase) * amplitude * ramp_tab[ramp];
|
||||
*baseband++ += sin(phase) * amplitude * ramp_tab[ramp];
|
||||
#endif
|
||||
ramp++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (length)
|
||||
goto again;
|
||||
|
||||
mod->phase = phase;
|
||||
mod->ramp = ramp;
|
||||
}
|
||||
|
||||
/* init FM demodulator */
|
||||
int 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 */
|
||||
iir_lowpass_init(&demod->lp[0], bandwidth / 2.0, samplerate, 2);
|
||||
iir_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");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* 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
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void fm_demod_exit(fm_demod_t *demod)
|
||||
{
|
||||
if (demod->sin_tab) {
|
||||
free(demod->sin_tab);
|
||||
demod->sin_tab = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/* do frequency demodulation of baseband and write them to samples */
|
||||
void fm_demodulate_complex(fm_demod_t *demod, sample_t *frequency, int length, float *baseband, sample_t *I, sample_t *Q)
|
||||
{
|
||||
double phase, rot, last_phase, dev, rate;
|
||||
double _sin, _cos;
|
||||
sample_t 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 < length; s++) {
|
||||
phase += rot;
|
||||
i = baseband[ss++];
|
||||
q = baseband[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;
|
||||
iir_process(&demod->lp[0], I, length);
|
||||
iir_process(&demod->lp[1], Q, length);
|
||||
last_phase = demod->last_phase;
|
||||
for (s = 0; s < length; 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;
|
||||
frequency[s] = dev;
|
||||
}
|
||||
demod->last_phase = last_phase;
|
||||
}
|
||||
|
||||
void fm_demodulate_real(fm_demod_t *demod, sample_t *frequency, int length, sample_t *baseband, sample_t *I, sample_t *Q)
|
||||
{
|
||||
double phase, rot, last_phase, dev, rate;
|
||||
double _sin, _cos;
|
||||
sample_t i;
|
||||
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 < length; s++) {
|
||||
phase += rot;
|
||||
i = baseband[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[s] = i * _sin;
|
||||
}
|
||||
demod->phase = phase;
|
||||
iir_process(&demod->lp[0], I, length);
|
||||
iir_process(&demod->lp[1], Q, length);
|
||||
last_phase = demod->last_phase;
|
||||
for (s = 0; s < length; 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;
|
||||
frequency[s] = dev;
|
||||
}
|
||||
demod->last_phase = last_phase;
|
||||
}
|
||||
|
||||
39
src/libfm/fm.h
Normal file
39
src/libfm/fm.h
Normal file
@@ -0,0 +1,39 @@
|
||||
#include "../libfilter/iir_filter.h"
|
||||
|
||||
enum fm_mod_state {
|
||||
MOD_STATE_OFF, /* transmitter off, no IQ vector */
|
||||
MOD_STATE_ON, /* transmitter on, FM modulated IQ vector */
|
||||
MOD_STATE_RAMP_UP, /* use half cos to ramp up IQ vector */
|
||||
MOD_STATE_RAMP_DOWN, /* use half cos to ramp down IQ vector */
|
||||
};
|
||||
|
||||
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 */
|
||||
enum fm_mod_state state;/* state of transmit power */
|
||||
double *ramp_tab; /* half cosine ramp up */
|
||||
int ramp; /* current ramp position */
|
||||
int ramp_length; /* number of values in ramp */
|
||||
} fm_mod_t;
|
||||
|
||||
int fm_mod_init(fm_mod_t *mod, double samplerate, double offset, double amplitude);
|
||||
void fm_mod_exit(fm_mod_t *mod);
|
||||
void fm_modulate_complex(fm_mod_t *mod, sample_t *frequency, uint8_t *power, int num, float *baseband);
|
||||
|
||||
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) */
|
||||
iir_filter_t lp[2]; /* filters received IQ signal */
|
||||
double *sin_tab; /* sine/cosine table rotation */
|
||||
} fm_demod_t;
|
||||
|
||||
int fm_demod_init(fm_demod_t *demod, double samplerate, double offset, double bandwidth);
|
||||
void fm_demod_exit(fm_demod_t *demod);
|
||||
void fm_demodulate_complex(fm_demod_t *demod, sample_t *frequency, int length, float *baseband, sample_t *I, sample_t *Q);
|
||||
void fm_demodulate_real(fm_demod_t *demod, sample_t *frequency, int length, sample_t *baseband, sample_t *I, sample_t *Q);
|
||||
|
||||
Reference in New Issue
Block a user