--- /dev/null
+/*---------------------------------------------------------------------------*\
+
+ 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 "fmfsk.h"
+#include "modem_probe.h"
+#include "comp_prim.h"
+
+#define STD_PROC_BITS 192
+
+
+/*
+ * Create a new fmfsk modem instance.
+ *
+ * int Fs - sample rate
+ * int Rb - non-manchester bitrate
+ * returns - new struct FMFSK on sucess, 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*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;
+
+ float *oldsamps = malloc(sizeof(float)*fmfsk->nmem);
+ if(oldsamps == NULL){
+ free(fmfsk);
+ return NULL;
+ }
+
+ fmfsk->oldsamps = oldsamps;
+
+ 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;
+}
+
+/*
+ * 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] = -1;
+ for(j=0; j<Ts; j++)
+ fmfsk_out[Ts+j+i*Ts] = 1;
+ } else {
+ /* Save a manchester-encoded 1 */
+ for(j=0; j<Ts; j++)
+ fmfsk_out[ j+i*Ts] = 1;
+ for(j=0; j<Ts; j++)
+ fmfsk_out[Ts+j+i*Ts] = -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;
+ int Ts = fmfsk->Ts;
+ int Fs = fmfsk->Ts;
+ 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;
+ int rx_timing,sample_offset;
+ int next_nin;
+ float apeven,apodd; /* Approx. prob of even or odd stream being correct */
+ float currv,mdiff,lastv;
+ uint8_t mbit;
+
+ /* Shift in nin samples */
+ memcpy(&oldsamps[0] , &oldsamps[nmem-nold], sizeof(float)*nold);
+ memcpy(&oldsamps[nold], &fmfsk_in[0] , sizeof(float)*nin );
+
+ /* Allocate memory for filtering */
+ float *rx_filt = alloca(sizeof(float)*nsym*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 = cos(2*M_PI*(float)Rs/(float)Fs);
+ dphi_ft.imag = sin(2*M_PI*(float)Rs/(float)Fs);
+
+ for(i=0; i<nsym*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);
+ }
+
+ /* Figure out the normalized RX timing, using David's magic number */
+ norm_rx_timing = atan2f(x.imag,x.real)/(2*M_PI) - .042;
+ rx_timing = (int)lroundf(norm_rx_timing*(float)Ts);
+
+ /* 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 > .5)
+ next_nin += Ts/2;
+ if(norm_rx_timing < .5)
+ 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;
+ apeven = 0;
+ apodd = 0;
+ for(i=0; i<nsym; i++){
+ /* Sample a filtered value */
+ currv = rx_filt[sample_offset+(i*Ts)];
+ mdiff = lastv - currv;
+ mbit = mdiff>0 ? 1 : 0;
+
+ /* 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;
+ }
+ }
+
+ 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 = rx_filt[sample_offset+((nsym-1)*Ts)];
+
+ modem_probe_samp_f("t_norm_rx_timing",&norm_rx_timing,1);
+ modem_probe_samp_f("t_rx_filt",rx_filt,nsym*Ts);
+}
--- /dev/null
+/*---------------------------------------------------------------------------*\
+
+ FILE........: fmfsk_mod.c
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 7 February 2016
+
+ C test driver for fmfsk_mod in fmfsk.c. Reads in a set of bits to modulate
+ from a file, passed as a parameter, and writes modulated output to
+ another file
+
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ 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 <stdio.h>
+#include <string.h>
+#include "fmfsk.h"
+#include "codec2_fdmdv.h"
+
+int main(int argc,char *argv[]){
+ struct FMFSK *fmfsk;
+ int Fs,Rb;
+ int i;
+ FILE *fin,*fout;
+ uint8_t *bitbuf;
+ int16_t *rawbuf;
+ float *modbuf;
+
+ if(argc<4){
+ fprintf(stderr,"usage: %s SampleFreq BitRate InputOneBitPerCharFile OutputModRawFile\n",argv[0]);
+ exit(1);
+ }
+
+ /* Extract parameters */
+ Fs = atoi(argv[1]);
+ Rb = atoi(argv[2]);
+
+ if(strcmp(argv[3],"-")==0){
+ fin = stdin;
+ }else{
+ fin = fopen(argv[3],"r");
+ }
+
+ if(strcmp(argv[4],"-")==0){
+ fout = stdout;
+ }else{
+ fout = fopen(argv[4],"w");
+ }
+
+
+ /* set up FMFSK */
+ fmfsk = fmfsk_create(Fs,Rb);
+
+ if(fin==NULL || fout==NULL || fmfsk==NULL){
+ fprintf(stderr,"Couldn't open test vector files\n");
+ goto cleanup;
+ }
+
+ /* allocate buffers for processing */
+ bitbuf = (uint8_t*)alloca(sizeof(uint8_t)*fmfsk->nbit);
+ rawbuf = (int16_t*)alloca(sizeof(int16_t)*fmfsk->N);
+ modbuf = (float*)alloca(sizeof(float)*fmfsk->N);
+
+ /* Modulate! */
+ while( fread(bitbuf,sizeof(uint8_t),fmfsk->nbit,fin) == fmfsk->nbit ){
+ fmfsk_mod(fmfsk,modbuf,bitbuf);
+ for(i=0; i<fmfsk->N; i++){
+ rawbuf[i] = (int16_t)(modbuf[i]*(float)FDMDV_SCALE);
+ }
+ fwrite(rawbuf,sizeof(int16_t),fmfsk->N,fout);
+
+ if(fin == stdin || fout == stdin){
+ fflush(fin);
+ fflush(fout);
+ }
+ }
+
+ cleanup:
+ fclose(fin);
+ fclose(fout);
+ fmfsk_destroy(fmfsk);
+ exit(0);
+}
projects / freetel-svn-tracking.git / commitdiff