From: baobrien <baobrien@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Sun, 7 Feb 2016 08:17:32 +0000 (+0000)
Subject: First commit of fmfsk C modem, not yet properly tested
X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=8f2fe4195aeabd7de54ddae239798a32df6c1a0f;p=freetel-svn-tracking.git

First commit of fmfsk C modem, not yet properly tested


git-svn-id: https://svn.code.sf.net/p/freetel/code@2689 01035d8c-6547-0410-b346-abe4f91aad63
---

diff --git a/codec2-dev/octave/fmfsk.m b/codec2-dev/octave/fmfsk.m
index c94f116c..37f748cc 100644
--- a/codec2-dev/octave/fmfsk.m
+++ b/codec2-dev/octave/fmfsk.m
@@ -27,20 +27,21 @@ pkg load parallel;
 %Fs is sample frequency
 %Rb is pre-manchester bit rate
 function states = fmfsk_init(Fs,Rb)
-    assert(floor(Fs/(Rb*2))==(Fs/(Rb*2)));
     assert(mod(Fs,Rb*2)==0);
     
+    %Current fixed processing buffer size, in non-ME bits
+    nbit = 192;
+    
     states.Rb = Rb;
     states.Rs = Rb*2;   % Manchester-encoded bitrate
     states.Fs = Fs;
     states.Ts = Fs/states.Rs;
-    % Processing buffer size. about 40ms here
-    states.N = floor(states.Rs*.080)*states.Ts;     
+    states.N = nbit*states.Ts;     
     states.nin = states.N;          % Samples in the next demod cycle
     states.nstash = states.Ts*2;    % How many samples to stash away between proc cycles for timing adjust
-    states.nmem =  states.N+(6*states.Ts);
-    states.nsym = floor(states.Rs*.080);
-    states.nbit = floor(states.Rb*.080);
+    states.nmem =  states.N+(4*states.Ts);
+    states.nsym = nbit*2;
+    states.nbit = nbit;
     %Old sample memory
     
     states.oldsamps = zeros(1,states.nmem);
@@ -101,7 +102,7 @@ function [rx_bits states] = fmfsk_demod(states,rx)
     % It's implemented like this for ease of C-porting
     for ii=(1:(nsym+1)*Ts)
         st = ii;
-        en = st+Ts-1;
+        en = st+Ts;
         rx_filt(ii) = sum(ssamps(st:en));
     end
  
diff --git a/codec2-dev/src/CMakeLists.txt b/codec2-dev/src/CMakeLists.txt
index c5211ec7..1ffbd9b3 100644
--- a/codec2-dev/src/CMakeLists.txt
+++ b/codec2-dev/src/CMakeLists.txt
@@ -142,6 +142,7 @@ set(CODEC2_SRCS
     fdmdv.c
     fm.c
     fsk.c
+    fmfsk.c
     kiss_fft.c
     kiss_fftr.c
     linreg.c
@@ -253,6 +254,9 @@ target_link_libraries(fm_demod ${CMAKE_REQUIRED_LIBRARIES})
 add_executable(cohpsk_mod cohpsk_mod.c)
 target_link_libraries(cohpsk_mod ${CMAKE_REQUIRED_LIBRARIES} codec2)
 
+add_executable(fmfsk_mod fmfsk_mod.c)
+target_link_libraries(fmfsk_mod ${CMAKE_REQUIRED_LIBRARIES} codec2)
+
 add_executable(cohpsk_demod cohpsk_demod.c octave.c)
 target_link_libraries(cohpsk_demod ${CMAKE_REQUIRED_LIBRARIES} codec2)
 
diff --git a/codec2-dev/src/fmfsk.c b/codec2-dev/src/fmfsk.c
new file mode 100644
index 00000000..57229b4a
--- /dev/null
+++ b/codec2-dev/src/fmfsk.c
@@ -0,0 +1,256 @@
+/*---------------------------------------------------------------------------*\
+
+  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);
+}
diff --git a/codec2-dev/src/fmfsk_mod.c b/codec2-dev/src/fmfsk_mod.c
new file mode 100644
index 00000000..69a632f7
--- /dev/null
+++ b/codec2-dev/src/fmfsk_mod.c
@@ -0,0 +1,99 @@
+/*---------------------------------------------------------------------------*\
+
+  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);
+}
diff --git a/codec2-dev/src/fsk.c b/codec2-dev/src/fsk.c
index 3539883e..92626eaa 100644
--- a/codec2-dev/src/fsk.c
+++ b/codec2-dev/src/fsk.c
@@ -4,7 +4,7 @@
   AUTHOR......: Brady O'Brien
   DATE CREATED: 7 January 2016
 
-  C Implementation of 2FSK modulator/demodulator, based on octave/fsk_horus.m
+  C Implementation of 2/4FSK modulator/demodulator, based on octave/fsk_horus.m
 
 \*---------------------------------------------------------------------------*/
 
diff --git a/codec2-dev/src/fsk.h b/codec2-dev/src/fsk.h
index bb92970d..a2c19815 100644
--- a/codec2-dev/src/fsk.h
+++ b/codec2-dev/src/fsk.h
@@ -4,7 +4,7 @@
   AUTHOR......: Brady O'Brien
   DATE CREATED: 6 January 2016
 
-  C Implementation of 2FSK modulator/demodulator, based on octave/fsk_horus.m
+  C Implementation of 2FSK/4FSK modulator/demodulator, based on octave/fsk_horus.m
 
 \*---------------------------------------------------------------------------*/