291 lines
8.7 KiB
C
291 lines
8.7 KiB
C
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FILE........: fm.c
|
||
|
|
AUTHOR......: David Rowe
|
||
|
|
DATE CREATED: February 2015
|
||
|
|
|
||
|
|
Functions that implement analog FM modulation and demodulation, see
|
||
|
|
also octave/fm.m.
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
/*
|
||
|
|
Copyright (C) 2015 David Rowe
|
||
|
|
|
||
|
|
All rights reserved.
|
||
|
|
|
||
|
|
This program is free software; you can redistribute it and/or modify
|
||
|
|
it under the terms of the GNU Lesser General Public License version 2.1, as
|
||
|
|
published by the Free Software Foundation. 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 Lesser General Public License
|
||
|
|
along with this program; if not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
DEFINES
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
#define FILT_MEM 200
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
INCLUDES
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
#include <assert.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include <math.h>
|
||
|
|
|
||
|
|
#include "codec2_fm.h"
|
||
|
|
#include "fm_fir_coeff.h"
|
||
|
|
#include "comp_prim.h"
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FUNCTIONS
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FUNCTION....: fm_create
|
||
|
|
AUTHOR......: David Rowe
|
||
|
|
DATE CREATED: 24 Feb 2015
|
||
|
|
|
||
|
|
Create and initialise an instance of the "modem". Returns a pointer
|
||
|
|
to the modem states or NULL on failure. One set of states is
|
||
|
|
sufficient for a full duplex modem.
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
struct FM *fm_create(int nsam)
|
||
|
|
{
|
||
|
|
struct FM *fm;
|
||
|
|
|
||
|
|
fm = (struct FM*)malloc(sizeof(struct FM));
|
||
|
|
if (fm == NULL)
|
||
|
|
return NULL;
|
||
|
|
fm->rx_bb = (COMP*)malloc(sizeof(COMP)*(FILT_MEM+nsam));
|
||
|
|
assert(fm->rx_bb != NULL);
|
||
|
|
|
||
|
|
fm->rx_bb_filt_prev.real = 0.0;
|
||
|
|
fm->rx_bb_filt_prev.imag = 0.0;
|
||
|
|
fm->lo_phase.real = 1.0;
|
||
|
|
fm->lo_phase.imag = 0.0;
|
||
|
|
|
||
|
|
fm->tx_phase = 0;
|
||
|
|
|
||
|
|
fm->rx_dem_mem = (float*)malloc(sizeof(float)*(FILT_MEM+nsam));
|
||
|
|
assert(fm->rx_dem_mem != NULL);
|
||
|
|
|
||
|
|
fm->nsam = nsam;
|
||
|
|
|
||
|
|
return fm;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void fm_destroy(struct FM *fm_states)
|
||
|
|
{
|
||
|
|
free(fm_states->rx_bb);
|
||
|
|
free(fm_states->rx_dem_mem);
|
||
|
|
free(fm_states);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FUNCTION....: fm_demod
|
||
|
|
AUTHOR......: David Rowe
|
||
|
|
DATE CREATED: 24 Feb 2015
|
||
|
|
|
||
|
|
Demodulate a FM signal to baseband audio.
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
void fm_demod(struct FM *fm_states, float rx_out[], float rx[])
|
||
|
|
{
|
||
|
|
float Fs = fm_states->Fs;
|
||
|
|
float fc = fm_states->fc;
|
||
|
|
float wc = 2*M_PI*fc/Fs;
|
||
|
|
float fd = fm_states->fd;
|
||
|
|
float wd = 2*M_PI*fd/Fs;
|
||
|
|
COMP *rx_bb = fm_states->rx_bb + FILT_MEM;
|
||
|
|
COMP wc_rect, rx_bb_filt, rx_bb_diff;
|
||
|
|
float rx_dem;
|
||
|
|
/*
|
||
|
|
float acc;
|
||
|
|
*/
|
||
|
|
float *rx_dem_mem = fm_states->rx_dem_mem + FILT_MEM;
|
||
|
|
int nsam = fm_states->nsam;
|
||
|
|
float mag;
|
||
|
|
int i,k;
|
||
|
|
|
||
|
|
wc_rect.real = cosf(wc); wc_rect.imag = -sinf(wc);
|
||
|
|
|
||
|
|
for(i=0; i<nsam; i++) {
|
||
|
|
|
||
|
|
/* down to complex baseband */
|
||
|
|
|
||
|
|
fm_states->lo_phase = cmult(fm_states->lo_phase, wc_rect);
|
||
|
|
rx_bb[i] = fcmult(rx[i], fm_states->lo_phase);
|
||
|
|
|
||
|
|
/* input FIR filter */
|
||
|
|
|
||
|
|
rx_bb_filt.real = 0.0; rx_bb_filt.imag = 0.0;
|
||
|
|
|
||
|
|
for(k=0; k<FILT_MEM/2; k++) {
|
||
|
|
rx_bb_filt.real += rx_bb[i-k].real * bin[k+FILT_MEM/4];
|
||
|
|
rx_bb_filt.imag += rx_bb[i-k].imag * bin[k+FILT_MEM/4];
|
||
|
|
}
|
||
|
|
|
||
|
|
//rx_bb_filt = rx_bb[i];
|
||
|
|
//printf("%f %f %f\n", rx[i], wc_rect.real, wc_rect.imag);
|
||
|
|
//printf("%f %f %f\n", rx[i], fm_states->lo_phase.real, fm_states->lo_phase.imag);
|
||
|
|
//printf("%f %f %f\n", rx[i], rx_bb[i].real, rx_bb[i].imag);
|
||
|
|
//printf("%f %f\n", rx_bb_filt.real, rx_bb_filt.imag);
|
||
|
|
/*
|
||
|
|
Differentiate first, in rect domain, then find angle, this
|
||
|
|
puts signal on the positive side of the real axis and helps
|
||
|
|
atan2() behaive.
|
||
|
|
*/
|
||
|
|
|
||
|
|
rx_bb_diff = cmult(rx_bb_filt, cconj(fm_states->rx_bb_filt_prev));
|
||
|
|
fm_states->rx_bb_filt_prev = rx_bb_filt;
|
||
|
|
|
||
|
|
rx_dem = atan2f(rx_bb_diff.imag, rx_bb_diff.real);
|
||
|
|
|
||
|
|
/* limit maximum phase jumps, to remove static type noise at low SNRs */
|
||
|
|
|
||
|
|
if (rx_dem > wd)
|
||
|
|
rx_dem = wd;
|
||
|
|
if (rx_dem < -wd)
|
||
|
|
rx_dem = -wd;
|
||
|
|
|
||
|
|
rx_dem *= (1/wd);
|
||
|
|
//printf("%f %f\n", rx_bb_diff.real, rx_bb_diff.imag);
|
||
|
|
rx_dem_mem[i] = rx_dem;
|
||
|
|
/*
|
||
|
|
acc = 0;
|
||
|
|
for(k=0; k<FILT_MEM; k++) {
|
||
|
|
acc += rx_dem_mem[i-k] * bout[k];
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
rx_out[i] = rx_dem;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* update filter memories */
|
||
|
|
|
||
|
|
rx_bb -= FILT_MEM;
|
||
|
|
rx_dem_mem -= FILT_MEM;
|
||
|
|
for(i=0; i<FILT_MEM; i++) {
|
||
|
|
rx_bb[i] = rx_bb[i+nsam];
|
||
|
|
rx_dem_mem[i] = rx_dem_mem[i+nsam];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* normalise digital oscillator as the magnitude can drift over time */
|
||
|
|
|
||
|
|
mag = cabsolute(fm_states->lo_phase);
|
||
|
|
fm_states->lo_phase.real /= mag;
|
||
|
|
fm_states->lo_phase.imag /= mag;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FUNCTION....: fm_mod
|
||
|
|
AUTHOR......: Brady O'Brien
|
||
|
|
DATE CREATED: Sept. 10 2015
|
||
|
|
|
||
|
|
Modulate an FM signal from a baseband modulating signal
|
||
|
|
|
||
|
|
struct FM *fm - FM state structure. Can be reused from fm_demod.
|
||
|
|
float tx_in[] - nsam baseband samples to be modulated
|
||
|
|
float tx_out[] - nsam samples in which to place the modulated FM
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
void fm_mod(struct FM *fm_states, float tx_in[], float tx_out[]) {
|
||
|
|
float Fs = fm_states->Fs; //Sampling freq
|
||
|
|
float fc = fm_states->fc; //Center freq
|
||
|
|
float wc = 2*M_PI*fc/Fs; //Center freq in rads/samp
|
||
|
|
float fd = fm_states->fd; //Max deviation in cycles/samp
|
||
|
|
float wd = 2*M_PI*fd/Fs; //Max deviation in rads/samp
|
||
|
|
int nsam = fm_states->nsam; //Samples per batch of modulation
|
||
|
|
float tx_phase = fm_states->tx_phase; //Transmit phase in rads
|
||
|
|
float w; //Temp variable for phase of VFO during loop
|
||
|
|
int i;
|
||
|
|
|
||
|
|
//Go through the samples, spin the oscillator, and generate some FM
|
||
|
|
for(i=0; i<nsam; i++){
|
||
|
|
w = wc + wd*tx_in[i]; //Calculate phase of VFO
|
||
|
|
tx_phase += w; //Spin TX oscillator
|
||
|
|
|
||
|
|
//TODO: Add pre-emphasis and pre-emph AGC for voice
|
||
|
|
|
||
|
|
//Make sure tx_phase stays from 0 to 2PI.
|
||
|
|
//If tx_phase goes above 4PI, It's because fc+fd*tx_in[i] is way too large for the sample
|
||
|
|
// rate.
|
||
|
|
if(tx_phase > 2*M_PI)
|
||
|
|
tx_phase -= 2*M_PI;
|
||
|
|
tx_out[i] = cosf(tx_phase);
|
||
|
|
}
|
||
|
|
//Save phase back into state struct
|
||
|
|
fm_states->tx_phase = tx_phase;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*---------------------------------------------------------------------------*\
|
||
|
|
|
||
|
|
FUNCTION....: fm_mod
|
||
|
|
AUTHOR......: Brady O'Brien
|
||
|
|
DATE CREATED: Sept. 10 2015
|
||
|
|
|
||
|
|
Modulate an FM signal from a baseband modulating signal. Output signal is
|
||
|
|
in complex domain
|
||
|
|
|
||
|
|
struct FM *fm - FM state structure. Can be reused from fm_demod.
|
||
|
|
float tx_in[] - nsam baseband samples to be modulated
|
||
|
|
COMP tx_out[] - nsam samples in which to place the modulated FM
|
||
|
|
|
||
|
|
\*---------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
void fm_mod_comp(struct FM *fm_states, float tx_in[], COMP tx_out[]){
|
||
|
|
float Fs = fm_states->Fs; //Sampling freq
|
||
|
|
float fc = fm_states->fc; //Center freq
|
||
|
|
float wc = 2*M_PI*fc/Fs; //Center freq in rads/samp
|
||
|
|
float fd = fm_states->fd; //Max deviation in cycles/samp
|
||
|
|
float wd = 2*M_PI*fd/Fs; //Max deviation in rads/samp
|
||
|
|
int nsam = fm_states->nsam; //Samples per batch of modulation
|
||
|
|
float tx_phase = fm_states->tx_phase; //Transmit phase in rads
|
||
|
|
float w; //Temp variable for phase of VFO during loop
|
||
|
|
int i;
|
||
|
|
|
||
|
|
//Go through the samples, spin the oscillator, and generate some FM
|
||
|
|
for(i=0; i<nsam; i++){
|
||
|
|
w = wc + wd*tx_in[i]; //Calculate phase of VFO
|
||
|
|
tx_phase += w; //Spin TX oscillator
|
||
|
|
|
||
|
|
//TODO: Add pre-emphasis and pre-emph AGC for voice
|
||
|
|
|
||
|
|
//Make sure tx_phase stays from 0 to 2PI.
|
||
|
|
//If tx_phase goes above 4PI, It's because fc+fd*tx_in[i] is way too large for the sample
|
||
|
|
// rate.
|
||
|
|
if(tx_phase > 2*M_PI)
|
||
|
|
tx_phase -= 2*M_PI;
|
||
|
|
|
||
|
|
tx_out[i].real = cosf(tx_phase);
|
||
|
|
tx_out[i].imag = sinf(tx_phase);
|
||
|
|
}
|
||
|
|
//Save phase back into state struct
|
||
|
|
fm_states->tx_phase = tx_phase;
|
||
|
|
}
|
||
|
|
|