diff options
Diffstat (limited to 'src/fmfsk.c')
| -rw-r--r-- | src/fmfsk.c | 373 |
1 files changed, 373 insertions, 0 deletions
diff --git a/src/fmfsk.c b/src/fmfsk.c new file mode 100644 index 0000000..442cd7c --- /dev/null +++ b/src/fmfsk.c @@ -0,0 +1,373 @@ +/*---------------------------------------------------------------------------*\ + + FILE........: fmfsk.c + AUTHOR......: Brady O'Brien + DATE CREATED: 6 February 2016 + + C Implementation of a FM+ME+FSK modem for FreeDV mode B and other applications + (better APRS, anyone?) + +\*---------------------------------------------------------------------------*/ + +/* + Copyright (C) 2016 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/>. +*/ + +#include <assert.h> +#include <stdint.h> +#include <stdlib.h> +#include <math.h> +#include <string.h> +#include <stdio.h> + + +#include "fmfsk.h" +#include "modem_probe.h" +#include "comp_prim.h" + +#define STD_PROC_BITS 96 + +/* + * Create a new fmfsk modem instance. + * + * int Fs - sample rate + * int Rb - non-manchester bitrate + * returns - new struct FMFSK on success, NULL on failure + */ +struct FMFSK * fmfsk_create(int Fs,int Rb){ + assert( Fs % (Rb*2) == 0 ); /* Sample freq must be divisible by symbol rate */ + + int nbits = STD_PROC_BITS; + + /* Allocate the struct */ + struct FMFSK *fmfsk = malloc(sizeof(struct FMFSK)); + if(fmfsk==NULL) return NULL; + + /* Set up static parameters */ + fmfsk->Rb = Rb; + fmfsk->Rs = Rb*2; + fmfsk->Fs = Fs; + fmfsk->Ts = Fs/fmfsk->Rs; + fmfsk->N = nbits*2*fmfsk->Ts; + fmfsk->nmem = fmfsk->N+(fmfsk->Ts*4); + fmfsk->nsym = nbits*2; + fmfsk->nbit = nbits; + + /* Set up demod state */ + fmfsk->lodd = 0; + fmfsk->nin = fmfsk->N; + fmfsk->snr_mean = 0; + + float *oldsamps = malloc(sizeof(float)*fmfsk->nmem); + if(oldsamps == NULL){ + free(fmfsk); + return NULL; + } + for(int i=0; i<fmfsk->nmem; i++) oldsamps[i] = 0.0; + fmfsk->oldsamps = oldsamps; + + fmfsk->stats = (struct MODEM_STATS*)malloc(sizeof(struct MODEM_STATS)); + if (fmfsk->stats == NULL) { + free(oldsamps); + free(fmfsk); + return NULL; + } + + return fmfsk; +} + +/* + * Destroys an fmfsk modem and deallocates memory + */ +void fmfsk_destroy(struct FMFSK *fmfsk){ + free(fmfsk->oldsamps); + free(fmfsk); +} + +/* + * Returns the number of samples that must be fed to fmfsk_demod the next + * cycle + */ +uint32_t fmfsk_nin(struct FMFSK *fmfsk){ + return (uint32_t)fmfsk->nin; +} + +void fmfsk_get_demod_stats(struct FMFSK *fmfsk,struct MODEM_STATS *stats){ + /* copy from internal stats, note we can't overwrite stats completely + as it has other states rqd by caller, also we want a consistent + interface across modem types for the freedv_api. + */ + + stats->clock_offset = fmfsk->stats->clock_offset; + stats->snr_est = fmfsk->stats->snr_est; // TODO: make this SNR not Eb/No + stats->rx_timing = fmfsk->stats->rx_timing; + stats->foff = fmfsk->stats->foff; + +#ifndef __EMBEDDED__ + stats->neyesamp = fmfsk->stats->neyesamp; + stats->neyetr = fmfsk->stats->neyetr; + memcpy(stats->rx_eye, fmfsk->stats->rx_eye, sizeof(stats->rx_eye)); +#endif // !__EMBEDDED__ + + /* these fields not used for FSK so set to something sensible */ + + stats->sync = 0; + stats->nr = fmfsk->stats->nr; + stats->Nc = fmfsk->stats->Nc; +} + +/* + * Modulates nbit bits into N samples to be sent through an FM radio + * + * struct FSK *fsk - FSK config/state struct, set up by fsk_create + * float mod_out[] - Buffer for N samples of modulated FMFSK + * uint8_t tx_bits[] - Buffer containing Nbits unpacked bits + */ + +void fmfsk_mod(struct FMFSK *fmfsk, float fmfsk_out[],uint8_t bits_in[]){ + int i,j; + int nbit = fmfsk->nbit; + int Ts = fmfsk->Ts; + + for(i=0; i<nbit; i++){ + /* Save a manchester-encoded 0 */ + if(bits_in[i] == 0){ + for(j=0; j<Ts; j++) + fmfsk_out[ j+i*Ts*2] = -1; + for(j=0; j<Ts; j++) + fmfsk_out[Ts+j+i*Ts*2] = 1; + } else { + /* Save a manchester-encoded 1 */ + for(j=0; j<Ts; j++) + fmfsk_out[ j+i*Ts*2] = 1; + for(j=0; j<Ts; j++) + fmfsk_out[Ts+j+i*Ts*2] = -1; + } + } +} + +/* + * Demodulate some number of FMFSK samples. The number of samples to be + * demodulated can be found by calling fmfsk_nin(). + * + * struct FMFSK *fsk - FMFSK config/state struct, set up by fsk_create + * uint8_t rx_bits[] - Buffer for nbit unpacked bits to be written + * float fsk_in[] - nin samples of modualted FMFSK from an FM radio + */ +void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[],float fmfsk_in[]){ + int i,j,k; + int Ts = fmfsk->Ts; + int Fs = fmfsk->Fs; + int Rs = fmfsk->Rs; + int nin = fmfsk->nin; + int N = fmfsk->N; + int nsym = fmfsk->nsym; + int nbit = fmfsk->nbit; + int nmem = fmfsk->nmem; + float *oldsamps = fmfsk->oldsamps; + int nold = nmem-nin; + COMP phi_ft,dphi_ft; /* Phase and delta-phase for fine timing estimator */ + float t; + COMP x; /* Magic fine timing angle */ + float norm_rx_timing,old_norm_rx_timing,d_norm_rx_timing,appm; + int rx_timing,sample_offset; + int next_nin; + float apeven,apodd; /* Approx. prob of even or odd stream being correct */ + float currv,mdiff,lastv; + int neyesamp; + int neyeoffset; + float eye_max; + uint8_t mbit; + float var_signal = 0, var_noise = 0, lastFabsV; + + /* Shift in nin samples */ + memmove(&oldsamps[0] , &oldsamps[nmem-nold], sizeof(float)*nold); + memcpy (&oldsamps[nold], &fmfsk_in[0] , sizeof(float)*nin ); + + /* Allocate memory for filtering */ + float *rx_filt = malloc(sizeof(float)*(nsym+1)*Ts); + + /* Integrate over Ts input symbols at every offset */ + for(i=0; i<(nsym+1)*Ts; i++){ + t=0; + /* Integrate over some samples */ + for(j=i;j<i+Ts;j++){ + t += oldsamps[j]; + } + rx_filt[i] = t; + } + + /* + * Fine timing estimation + * + * Estimate fine timing using line at Rs/2 that Manchester encoding provides + * We need this to sync up to Manchester codewords. + */ + + /* init fine timing extractor */ + phi_ft.real = 1; + phi_ft.imag = 0; + + /* Set up delta-phase */ + dphi_ft.real = cosf(2*M_PI*((float)Rs)/((float)Fs)); + dphi_ft.imag = sinf(2*M_PI*((float)Rs)/((float)Fs)); + + x.real = 0; + x.imag = 0; + + for(i=0; i<(nsym+1)*Ts; i++){ + /* Apply non-linearity */ + t = rx_filt[i]*rx_filt[i]; + + /* Shift Rs/2 down to DC and accumulate */ + x = cadd(x,fcmult(t,phi_ft)); + + /* Spin downshift oscillator */ + phi_ft = cmult(dphi_ft,phi_ft); + modem_probe_samp_c("t_phi_ft",&phi_ft,1); + } + + /* Figure out the normalized RX timing, using David's magic number */ + norm_rx_timing = atan2f(x.imag,x.real)/(2*M_PI) - .42; + rx_timing = (int)lroundf(norm_rx_timing*(float)Ts); + + old_norm_rx_timing = fmfsk->norm_rx_timing; + fmfsk->norm_rx_timing = norm_rx_timing; + + /* Estimate sample clock offset */ + d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing; + + /* Filter out big jumps in due to nin change */ + if(fabsf(d_norm_rx_timing) < .2){ + appm = 1e6*d_norm_rx_timing/(float)nsym; + fmfsk->ppm = .9*fmfsk->ppm + .1*appm; + } + + /* Figure out how far offset the sample points are */ + sample_offset = (Ts/2)+Ts+rx_timing-1; + + /* Request fewer or greater samples next time, if fine timing is far + * enough off. This also makes it possible to tolerate clock offsets */ + next_nin = N; + if(norm_rx_timing > -.2) + next_nin += Ts/2; + if(norm_rx_timing < -.65) + next_nin -= Ts/2; + fmfsk->nin = next_nin; + + /* Make first diff of this round the last sample of the last round, + * for the odd stream */ + lastv = fmfsk->lodd; + lastFabsV = fabs(lastv); + apeven = 0; + apodd = 0; + for(i=0; i<nsym; i++){ + /* Sample a filtered value */ + currv = rx_filt[sample_offset+(i*Ts)]; + modem_probe_samp_f("t_symsamp",&currv,1); + mdiff = lastv - currv; + mbit = mdiff>0 ? 1 : 0; + lastv = currv; + + // Calculate the signal variance. Note that the mean is zero + var_signal += currv * currv; + + /* Calculate the variance of the noise between samples (symbols). A quick variance estimate + * without calculating mean can be done by differentiating (remove mean) and then + * dividing by 2. Fabs the samples as we are looking at how close the samples are to each + * other as if they were all the same polarity/symbol. */ + currv = fabs(currv); + var_noise += (currv - lastFabsV) * (currv - lastFabsV); + lastFabsV = currv; + + mdiff = mdiff>0 ? mdiff : 0-mdiff; + + /* Put bit in it's stream */ + if((i%2)==1){ + apeven += mdiff; + /* Even stream goes in LSB */ + rx_bits[i>>1] |= mbit ? 0x1 : 0x0; + }else{ + apodd += mdiff; + /* Odd in second-to-LSB */ + rx_bits[i>>1] = mbit ? 0x2 : 0x0; + } + } + + /* Div by 2 to correct variance when doing via differentiation.*/ + var_noise *= 0.5; + + if(apeven>apodd){ + /* Zero out odd bits from output bitstream */ + for(i=0;i<nbit;i++) + rx_bits[i] &= 0x1; + }else{ + /* Shift odd bits into LSB and even bits out of existence */ + for(i=0;i<nbit;i++) + rx_bits[i] = (rx_bits[i]&0x2)>>1; + } + + /* Save last sample of int stream for next demod round */ + fmfsk->lodd = lastv; + + /* Save demod statistics */ + fmfsk->stats->Nc = 0; + fmfsk->stats->nr = 0; + + /* Clock offset and RX timing are all we know here */ + fmfsk->stats->clock_offset = fmfsk->ppm; + fmfsk->stats->rx_timing = (float)rx_timing; + + /* Zero out all of the other things */ + fmfsk->stats->foff = 0; + + /* Use moving average to smooth SNR */ + var_signal += 1E-6/3.1; var_noise += 1E-6; /* prevent NAN and bias towards -5dB SNR for zero signal inputs */ + if(fmfsk->snr_mean < 0.1) + fmfsk->snr_mean = (10.0 * log10f(var_signal / var_noise)); + else + fmfsk->snr_mean = 0.9 * fmfsk->snr_mean + 0.1 * (10.0 * log10f(var_signal / var_noise)); + fmfsk->stats->snr_est = fmfsk->snr_mean; + +#ifndef __EMBEDDED__ + /* Collect an eye diagram */ + /* Take a sample for the eye diagrams */ + neyesamp = fmfsk->stats->neyesamp = Ts*4; + neyeoffset = sample_offset+(Ts*2*28); + + fmfsk->stats->neyetr = 8; + for(k=0; k<fmfsk->stats->neyetr; k++) + for(j=0; j<neyesamp; j++) + fmfsk->stats->rx_eye[k][j] = rx_filt[k*neyesamp+neyeoffset+j]; + //fmfsk->stats->rx_eye[k][j] = fmfsk_in[k*neyesamp+neyeoffset+j]; + eye_max = 0; + + /* Normalize eye to +/- 1 */ + for(i=0; i<fmfsk->stats->neyetr; i++) + for(j=0; j<neyesamp; j++) + if(fabsf(fmfsk->stats->rx_eye[i][j])>eye_max) + eye_max = fabsf(fmfsk->stats->rx_eye[i][j]); + + for(i=0; i<fmfsk->stats->neyetr; i++) + for(j=0; j<neyesamp; j++) + fmfsk->stats->rx_eye[i][j] = (fmfsk->stats->rx_eye[i][j]/(2*eye_max))+.5; +#endif // !__EMBEDDED__ + + modem_probe_samp_f("t_norm_rx_timing",&norm_rx_timing,1); + modem_probe_samp_f("t_rx_filt",rx_filt,(nsym+1)*Ts); + + free(rx_filt); +} |