From 724587f04a9b6f250ebe98d820d1e9f07f7a13ce Mon Sep 17 00:00:00 2001
From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Thu, 15 Dec 2016 19:43:10 +0000
Subject: [PATCH] thanks Jeroen for a patch to implement SNR on 2400B and fix
 biug introduced by complex interface to fsk modem

git-svn-id: https://svn.code.sf.net/p/freetel/code@2933 01035d8c-6547-0410-b346-abe4f91aad63
---
 codec2-dev/src/fm.c         |  5 ++---
 codec2-dev/src/fmfsk.c      | 33 +++++++++++++++++++++++++++++++++
 codec2-dev/src/fmfsk.h      |  1 +
 codec2-dev/src/freedv_api.c | 18 +++++++++---------
 4 files changed, 45 insertions(+), 12 deletions(-)

diff --git a/codec2-dev/src/fm.c b/codec2-dev/src/fm.c
index 16f3ef15..e1fbe37b 100644
--- a/codec2-dev/src/fm.c
+++ b/codec2-dev/src/fm.c
@@ -213,7 +213,7 @@ void fm_demod(struct FM *fm_states, float rx_out[], float rx[])
 
 \*---------------------------------------------------------------------------*/
 
-void fm_mod(struct FM *fm_states, float tx_in[], float tx_out[]){
+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
@@ -236,7 +236,6 @@ void fm_mod(struct FM *fm_states, float tx_in[], float tx_out[]){
       // rate.
       if(tx_phase > 2*M_PI)
           tx_phase -= 2*M_PI;
-
       tx_out[i] = cosf(tx_phase);
   }
   //Save phase back into state struct
@@ -273,7 +272,7 @@ void fm_mod_comp(struct FM *fm_states, float tx_in[], COMP tx_out[]){
   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.
diff --git a/codec2-dev/src/fmfsk.c b/codec2-dev/src/fmfsk.c
index 054e0a26..1c89179c 100644
--- a/codec2-dev/src/fmfsk.c
+++ b/codec2-dev/src/fmfsk.c
@@ -40,6 +40,9 @@
 
 #define STD_PROC_BITS 96
 
+/* Define this to enable EbNodB estimate */
+/* This needs square roots, may take more cpu time than it's worth */
+#define EST_EBNO
 
 /*
  * Create a new fmfsk modem instance.
@@ -70,6 +73,7 @@ struct FMFSK * fmfsk_create(int Fs,int Rb){
     /* 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){
@@ -165,6 +169,9 @@ void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[],float fmfsk_in[]){
     int neyeoffset;
     float eye_max;
     uint8_t mbit;
+    #ifdef EST_EBNO
+    float amp_even = 0, amp_odd = 0, amp_bit, amp_noise;
+    #endif
     
     /* Shift in nin samples */
     memcpy(&oldsamps[0]   , &oldsamps[nmem-nold], sizeof(float)*nold);
@@ -261,20 +268,38 @@ void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[],float fmfsk_in[]){
             apeven += mdiff;
             /* Even stream goes in LSB */
             rx_bits[i>>1] |= mbit ? 0x1 : 0x0;
+            #ifdef EST_EBNO
+            amp_even += currv * currv;
+            #endif
         }else{
             apodd += mdiff;
             /* Odd in second-to-LSB */
             rx_bits[i>>1]  = mbit ? 0x2 : 0x0;
+            #ifdef EST_EBNO
+            amp_odd += currv * currv;
+            #endif
         }
     }
+    #ifdef EST_EBNO
+    amp_even = sqrt(amp_even);
+    amp_odd = sqrt(amp_odd);
+    #endif
     if(apeven>apodd){
         /* Zero out odd bits from output bitstream */
         for(i=0;i<nbit;i++)
             rx_bits[i] &= 0x1;
+        #ifdef EST_EBNO
+        amp_bit = amp_even;
+        amp_noise = amp_odd;
+        #endif
     }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;
+        #ifdef EST_EBNO
+        amp_bit = amp_odd;
+        amp_noise = amp_even;
+        #endif
     }
     
     /* Save last sample of int stream for next demod round */
@@ -291,7 +316,15 @@ void fmfsk_demod(struct FMFSK *fmfsk, uint8_t rx_bits[],float fmfsk_in[]){
         
         /* Zero out all of the other things */
         fmfsk->stats->foff = 0;
+
+        #ifdef EST_EBNO
+        amp_bit = fabsf(amp_bit - amp_noise);
+        fmfsk->snr_mean *= .9;
+        fmfsk->snr_mean += (amp_bit+1e-6)/(amp_noise+1e-6);
+        fmfsk->stats->snr_est = 20+20*log10f(fmfsk->snr_mean); 
+        #else
         fmfsk->stats->snr_est = 0;
+        #endif
         
         /* Collect an eye diagram */
         /* Take a sample for the eye diagrams */
diff --git a/codec2-dev/src/fmfsk.h b/codec2-dev/src/fmfsk.h
index 9f7cf06d..dd04ccd7 100644
--- a/codec2-dev/src/fmfsk.h
+++ b/codec2-dev/src/fmfsk.h
@@ -56,6 +56,7 @@ struct FMFSK{
     /* Stats generated by demod */
     float norm_rx_timing; /* RX Timing, used to calculate clock offset */
     int ppm;			/* Clock offset in parts-per-million */
+    float snr_mean;
     
     /* Modem stat structure */
     struct MODEM_STATS * stats;
diff --git a/codec2-dev/src/freedv_api.c b/codec2-dev/src/freedv_api.c
index 81d3f336..4e0db6d6 100644
--- a/codec2-dev/src/freedv_api.c
+++ b/codec2-dev/src/freedv_api.c
@@ -867,17 +867,17 @@ int freedv_floatrx_fsk(struct freedv *f, float demod_in[], int *valid) {
     int n_ascii;
     char ascii_out;
 
-    /* DR: 21/11/16 - temp code while porting fsk_demod to complex */
-
-    int n = fsk_nin(f->fsk);
-    COMP demod_comp[n];
-    for(i=0; i<n; i++) {
-        demod_comp[i].real = demod_in[i];
-        demod_comp[i].imag = 0.0;;        
-    }
 
     if(f->mode == FREEDV_MODE_2400A || f->mode == FREEDV_MODE_800XA){
-        fsk_demod(f->fsk,(uint8_t*)f->tx_bits,demod_comp);
+        /* DR: 21/11/16 - temp code while porting fsk_demod to complex */
+        int n = fsk_nin(f->fsk);
+        COMP demod_comp[n];
+        for(i=0; i<n; i++) {
+            demod_comp[i].real = demod_in[i];
+            demod_comp[i].imag = 0.0;;        
+        }
+        
+	fsk_demod(f->fsk,(uint8_t*)f->tx_bits,demod_comp);
         f->nin = fsk_nin(f->fsk);
     }else{            
         fmfsk_demod(f->fmfsk,(uint8_t*)f->tx_bits,demod_in);
-- 
2.25.1