From 82c9ff2850235a69206a1fb2db3a9f039608a6f6 Mon Sep 17 00:00:00 2001
From: Manolis Surligas <surligas@csd.uoc.gr>
Date: Thu, 31 Mar 2016 21:58:29 +0300
Subject: [PATCH] Fix the doppler correction mechanism

The doppler correction mechanism has successfully passed the first tests
using the GPredict software for the radio control. The corrections seems
to be quite smooth. The next step is to apply it at an audible signal.
---
 grc/satnogs_doppler_correction_cc.xml | 77 +++++++++++++++------------
 lib/doppler_correction_cc_impl.cc     | 36 +++++++------
 2 files changed, 65 insertions(+), 48 deletions(-)

diff --git a/grc/satnogs_doppler_correction_cc.xml b/grc/satnogs_doppler_correction_cc.xml
index 084bb41..63840f8 100644
--- a/grc/satnogs_doppler_correction_cc.xml
+++ b/grc/satnogs_doppler_correction_cc.xml
@@ -1,38 +1,49 @@
 <?xml version="1.0"?>
 <block>
-  <name>doppler_correction_cc</name>
-  <key>satnogs_doppler_correction_cc</key>
-  <category>satnogs</category>
-  <import>import satnogs</import>
-  <make>satnogs.doppler_correction_cc($target_freq)</make>
-  <!-- Make one 'param' node for every Parameter you want settable from the GUI.
-       Sub-nodes:
-       * name
-       * key (makes the value accessible as $keyname, e.g. in the make node)
-       * type -->
-  <param>
-    <name>...</name>
-    <key>...</key>
-    <type>...</type>
-  </param>
+	<name>Doppler Correction</name>
+	<key>satnogs_doppler_correction_cc</key>
+	<category>satnogs</category>
+	<import>import satnogs</import>
+	<make>satnogs.doppler_correction_cc($target_freq, $sampling_rate, $corrections_per_sec)</make>
 
-  <!-- Make one 'sink' node per input. Sub-nodes:
-       * name (an identifier for the GUI)
-       * type
-       * vlen
-       * optional (set to 1 for optional inputs) -->
-  <sink>
-    <name>in</name>
-    <type><!-- e.g. int, float, complex, byte, short, xxx_vector, ...--></type>
-  </sink>
+	<param>
+		<name>Target Frequency</name>
+		<key>target_freq</key>
+		<type>real</type>
+	</param>
+	
+	<param>
+		<name>Sample Rate</name>
+		<key>sampling_rate</key>
+		<value>samp_rate</value>
+		<type>real</type>
+	</param>
+	
+	<param>
+		<name>Corrections per Second</name>
+		<key>corrections_per_sec</key>
+		<value>1000</value>
+		<type>int</type>
+	</param>
 
-  <!-- Make one 'source' node per output. Sub-nodes:
-       * name (an identifier for the GUI)
-       * type
-       * vlen
-       * optional (set to 1 for optional inputs) -->
-  <source>
-    <name>out</name>
-    <type><!-- e.g. int, float, complex, byte, short, xxx_vector, ...--></type>
-  </source>
+	<sink>
+		<name>freq</name>
+		<type>message</type>
+	</sink>
+
+	<sink>
+		<name>reset</name>
+		<type>message</type>
+		<optional>1</optional>
+	</sink>
+
+	<sink>
+		<name>in</name>
+		<type>complex</type>
+	</sink>
+
+	<source>
+		<name>out</name>
+		<type>complex</type>
+	</source>
 </block>
diff --git a/lib/doppler_correction_cc_impl.cc b/lib/doppler_correction_cc_impl.cc
index d877dd5..2d1525b 100644
--- a/lib/doppler_correction_cc_impl.cc
+++ b/lib/doppler_correction_cc_impl.cc
@@ -24,6 +24,7 @@
 
 #include <gnuradio/io_signature.h>
 #include "doppler_correction_cc_impl.h"
+#include <satnogs/log.h>
 #include <volk/volk.h>
 
 namespace gr
@@ -75,7 +76,7 @@ namespace gr
        * and the input message handler are NOT reentrant.
        */
       set_max_noutput_items (d_samp_rate / 2.0);
-      set_alignment(8);
+      set_alignment (8);
 
       set_msg_handler (
 	  pmt::mp ("freq"),
@@ -104,11 +105,15 @@ namespace gr
       d_freq_diff = d_target_freq - new_freq;
       if (!d_have_est) {
 	d_freq_est_num++;
-	if (d_freq_est_num > d_est_thrhld - 1) {
-	  d_have_est = true;
-	}
 	d_doppler_freqs.push_back (
 	    freq_drift (nitems_written (0), d_freq_diff));
+	if (d_freq_est_num > d_est_thrhld - 1) {
+	  d_doppler_fit_engine.fit (d_doppler_freqs);
+	  d_doppler_fit_engine.predict_freqs (d_predicted_freqs,
+					      d_corrections_per_sec,
+					      d_update_period);
+	  d_have_est = true;
+	}
       }
       else {
 	d_doppler_freqs.pop_front ();
@@ -121,6 +126,7 @@ namespace gr
 	d_doppler_fit_engine.predict_freqs (d_predicted_freqs,
 					    d_corrections_per_sec,
 					    d_update_period);
+	d_corrections = 0;
       }
     }
 
@@ -180,20 +186,20 @@ namespace gr
 						  d_update_period);
 	      d_corrections = 0;
 	    }
+	  }
 
-	    cnt = std::min(d_update_period - d_corrected_samples,
-			   (size_t) (noutput_items - produced));
-	    /* Perform the doppler shift correction */
-	    volk_32fc_x2_multiply_32fc (out + produced, in + produced,
-					d_nco_buff + d_corrected_samples, cnt);
+	  cnt = std::min (d_update_period - d_corrected_samples,
+			  (size_t) (noutput_items - produced));
+	  /* Perform the doppler shift correction */
+	  volk_32fc_x2_multiply_32fc (out + produced, in + produced,
+				      d_nco_buff + d_corrected_samples, cnt);
 
-	    /* Make the proper advances */
-	    produced += (int) cnt;
-	    d_corrected_samples += cnt;
+	  /* Make the proper advances */
+	  produced += (int) cnt;
+	  d_corrected_samples += cnt;
 
-	    if(d_corrected_samples == d_update_period) {
-	      d_corrected_samples = 0;
-	    }
+	  if (d_corrected_samples == d_update_period) {
+	    d_corrected_samples = 0;
 	  }
 	}
       }