From eac7cbb86c5c8776f3fbddee0c4f4a86859cfe84 Mon Sep 17 00:00:00 2001
From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Wed, 21 Jun 2017 21:21:39 +0000
Subject: [PATCH] small constant to prevent instability in freq offset est
 atan()

git-svn-id: https://svn.code.sf.net/p/freetel/code@3234 01035d8c-6547-0410-b346-abe4f91aad63
---
 codec2-dev/octave/ofdm_lib.m | 6 ++++++
 codec2-dev/octave/tofdm.m    | 2 +-
 codec2-dev/src/ofdm.c        | 8 +++++++-
 codec2-dev/unittest/tofdm.c  | 2 +-
 4 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/codec2-dev/octave/ofdm_lib.m b/codec2-dev/octave/ofdm_lib.m
index 323fbfde..6d6b827d 100644
--- a/codec2-dev/octave/ofdm_lib.m
+++ b/codec2-dev/octave/ofdm_lib.m
@@ -331,6 +331,12 @@ function [rx_bits states aphase_est_pilot_log rx_np rx_amp] = ofdm_demod(states,
       
   if foff_est_en
     freq_err_rect = sum(rx_sym(2,:))' * sum(rx_sym(2+Ns,:));
+
+    % prevent instability in atan(im/re) when real part near 0 
+
+    freq_err_rect += 1E-6;
+
+    %printf("freq_err_rect: %f %f angle: %f\n", real(freq_err_rect), imag(freq_err_rect), angle(freq_err_rect));
     freq_err_hz = angle(freq_err_rect)*Rs/(2*pi*Ns);
     foff_est_hz = foff_est_hz + foff_est_gain*freq_err_hz;
   end
diff --git a/codec2-dev/octave/tofdm.m b/codec2-dev/octave/tofdm.m
index 63391aac..0abb1780 100644
--- a/codec2-dev/octave/tofdm.m
+++ b/codec2-dev/octave/tofdm.m
@@ -47,7 +47,7 @@ phase_est_pilot_log = []; rx_amp_log = [];
 rx_np_log = []; rx_bits_log = [];
 
 states.timing_en = 1;
-states.foff_est_en = 0;
+states.foff_est_en = 1;
 states.phase_est_en = 1;
 
 if states.timing_en == 0
diff --git a/codec2-dev/src/ofdm.c b/codec2-dev/src/ofdm.c
index 7732a5ac..ce3eb18d 100644
--- a/codec2-dev/src/ofdm.c
+++ b/codec2-dev/src/ofdm.c
@@ -539,7 +539,13 @@ void ofdm_demod(struct OFDM *ofdm, int *rx_bits, COMP *rxbuf_in) {
     /* est freq err based on all carriers ------------------------------------ */
 
     if (ofdm->foff_est_en == true) {
-        complex float freq_err_rect = conjf(vector_sum(ofdm->rx_sym[1], OFDM_NC)) * vector_sum(ofdm->rx_sym[OFDM_NS + 1], OFDM_NC);
+        complex float freq_err_rect = conjf(vector_sum(ofdm->rx_sym[1], OFDM_NC+2)) * vector_sum(ofdm->rx_sym[OFDM_NS + 1], OFDM_NC+2);
+
+        /* prevent instability in atan(im/re) when real part near 0 */
+
+        freq_err_rect += 1E-6;
+
+        // fprintf(stderr, "freq_err_rect: %f %f angle: %f\n", crealf(freq_err_rect), cimagf(freq_err_rect),cargf(freq_err_rect) );
         freq_err_hz = cargf(freq_err_rect) * OFDM_RS / (TAU * OFDM_NS);
 
         ofdm->foff_est_hz += (ofdm->foff_est_gain * freq_err_hz);
diff --git a/codec2-dev/unittest/tofdm.c b/codec2-dev/unittest/tofdm.c
index 10088039..78638ea2 100644
--- a/codec2-dev/unittest/tofdm.c
+++ b/codec2-dev/unittest/tofdm.c
@@ -116,7 +116,7 @@ int main(int argc, char *argv[])
 
     ofdm_set_verbose(ofdm, true);
     ofdm_set_timing_enable(ofdm, true);
-    ofdm_set_foff_est_enable(ofdm, false);
+    ofdm_set_foff_est_enable(ofdm, true);
     ofdm_set_phase_est_enable(ofdm, true);
 
     for(f=0; f<NFRAMES; f++) {
-- 
2.25.1