Continue working on the Doppler Correction block

Actually the block seems to be ready, however proper testing is
necessary

include/satnogs/doppler_fit.h

@@ -24,6 +24,7 @@
 #include <satnogs/api.h>
 #include <satnogs/freq_drift.h>
 #include <deque>
+#include <boost/thread/mutex.hpp>
 
 namespace gr
 {
@@ -38,13 +39,19 @@ namespace gr
     {
     public:
       doppler_fit (size_t degree);
-      ~doppler_fit ();
 
-      void fit(std::deque<freq_drift> drifts);
+      void
+      fit (std::deque<freq_drift> drifts);
+
+      void
+      predict_freqs (double *freqs, size_t ncorrections,
+		     size_t samples_per_correction);
 
     private:
       const size_t d_degree;
+      double d_last_x;
       std::vector<double> d_coeff;
+      boost::mutex d_mutex;
     };
 
   } // namespace satnogs

lib/doppler_correction_cc_impl.cc

@@ -24,6 +24,7 @@
 
 #include <gnuradio/io_signature.h>
 #include "doppler_correction_cc_impl.h"
+#include <volk/volk.h>
 
 namespace gr
 {
@@ -50,29 +51,48 @@ namespace gr
 	    d_target_freq (target_freq),
 	    d_samp_rate (sampling_rate),
 	    d_update_period (sampling_rate / (double) corrections_per_sec),
-	    d_est_thrhld(7),
+	    d_est_thrhld (7),
+	    d_corrections_per_sec (corrections_per_sec),
+	    d_nco (),
+	    /* A 3-rd order polynomial curve fitting is more than enough */
+	    d_doppler_fit_engine (3),
 	    d_freq_diff (0.0),
-	    d_have_est(false),
-	    d_freq_est_num(0)
+	    d_have_est (false),
+	    d_freq_est_num (0),
+	    d_corrections (0),
+	    d_corrected_samples (0)
     {
       message_port_register_in (pmt::mp ("freq"));
       message_port_register_in (pmt::mp ("reset"));
+
       /*
+       * NOTE:
        * Set the maximum number of samples to be equivalent of half a second.
        * With this way we are sure that at least one frequency message
        * per second will be processed.
        *
        * This is taken into consideration due to the fact that the work()
-       * and the input message handler are not reentrant.
+       * and the input message handler are NOT reentrant.
        */
       set_max_noutput_items (d_samp_rate / 2.0);
+      set_alignment(8);
 
-      set_msg_handler(pmt::mp("freq"),
-		      boost::bind(&doppler_correction_cc_impl::new_freq,
-				  this, _1));
-      set_msg_handler(pmt::mp("reset"),
-		      boost::bind(&doppler_correction_cc_impl::reset,
-				  this, _1));
+      set_msg_handler (
+	  pmt::mp ("freq"),
+	  boost::bind (&doppler_correction_cc_impl::new_freq, this, _1));
+      set_msg_handler (
+	  pmt::mp ("reset"),
+	  boost::bind (&doppler_correction_cc_impl::reset, this, _1));
+
+      /* Allocate the buffer that will hold the predicted frequency differences */
+      d_predicted_freqs = new double[d_corrections_per_sec];
+
+      /* Allocate aligned memory for the NCO */
+      d_nco_buff = (gr_complex *) volk_malloc (
+	  d_update_period * sizeof(gr_complex), 32);
+      if (!d_nco_buff) {
+	throw std::runtime_error ("Could not allocate NCO memory");
+      }
     }
 
     void
@@ -80,18 +100,27 @@ namespace gr
     {
       boost::mutex::scoped_lock lock (d_mutex);
       double new_freq;
-      new_freq = pmt::to_double(msg);
+      new_freq = pmt::to_double (msg);
       d_freq_diff = d_target_freq - new_freq;
-      if(!d_have_est){
+      if (!d_have_est) {
 	d_freq_est_num++;
-	if(d_freq_est_num > d_est_thrhld - 1){
+	if (d_freq_est_num > d_est_thrhld - 1) {
 	  d_have_est = true;
 	}
-	d_doppler_freqs.push_back(d_freq_diff);
+	d_doppler_freqs.push_back (
+	    freq_drift (nitems_written (0), d_freq_diff));
       }
-      else{
-	d_doppler_freqs.pop_front();
-	d_doppler_freqs.push_back(d_freq_diff);
+      else {
+	d_doppler_freqs.pop_front ();
+	d_doppler_freqs.push_back (
+	    freq_drift (nitems_written (0), d_freq_diff));
+
+	/* Fit the doppler drift based on the new estimated frequency */
+	d_doppler_fit_engine.fit (d_doppler_freqs);
+	/* Predict the frequency differences for the near future */
+	d_doppler_fit_engine.predict_freqs (d_predicted_freqs,
+					    d_corrections_per_sec,
+					    d_update_period);
       }
     }
 
@@ -99,8 +128,9 @@ namespace gr
     doppler_correction_cc_impl::reset (pmt::pmt_t msg)
     {
       boost::mutex::scoped_lock lock (d_mutex);
-      d_doppler_freqs.clear();
+      d_doppler_freqs.clear ();
       d_freq_est_num = 0;
+      d_corrections = 0;
       d_have_est = false;
     }
 
@@ -109,6 +139,8 @@ namespace gr
      */
     doppler_correction_cc_impl::~doppler_correction_cc_impl ()
     {
+      delete[] d_predicted_freqs;
+      volk_free (d_nco_buff);
     }
 
     int
@@ -118,16 +150,55 @@ namespace gr
     {
       const gr_complex *in = (const gr_complex *) input_items[0];
       gr_complex *out = (gr_complex *) output_items[0];
+      int produced = 0;
+      size_t cnt;
 
       /*
        * If we do not have an estimation yet, just copy the input to the output.
        * Otherwise perform Doppler correction, using the fitted curve indicating
        * the frequency drift.
        */
-      if(d_have_est){
+      if (d_have_est) {
+	while (produced < noutput_items) {
+	  /*
+	   * If no samples have been corrected from the current correction step
+	   * compute and store the NCO buffer with the corresponding frequency
+	   */
+	  if (d_corrected_samples == 0) {
+	    d_nco.set_freq (
+		2 * M_PI * (-d_predicted_freqs[d_corrections]) / d_samp_rate);
+	    d_nco.sincos (d_nco_buff, d_update_period, 1.0);
+	    d_corrections++;
+
+	    /*
+	     * The doppler estimation may fail/delay. In such a case the block
+	     * should continue using the predicted frequencies
+	     */
+	    if (d_corrections == d_corrections_per_sec) {
+	      d_doppler_fit_engine.predict_freqs (d_predicted_freqs,
+						  d_corrections_per_sec,
+						  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);
+
+	    /* Make the proper advances */
+	    produced += (int) cnt;
+	    d_corrected_samples += cnt;
+
+	    if(d_corrected_samples == d_update_period) {
+	      d_corrected_samples = 0;
+	    }
+	  }
+	}
       }
-      else{
-	memcpy(out, in, noutput_items * sizeof(gr_complex));
+      else {
+	memcpy (out, in, noutput_items * sizeof(gr_complex));
       }
 
       return noutput_items;

lib/doppler_correction_cc_impl.h

@@ -22,6 +22,9 @@
 #define INCLUDED_SATNOGS_DOPPLER_CORRECTION_CC_IMPL_H
 
 #include <satnogs/doppler_correction_cc.h>
+#include <satnogs/freq_drift.h>
+#include <satnogs/doppler_fit.h>
+#include <gnuradio/fxpt_nco.h>
 #include <deque>
 
 namespace gr
@@ -34,13 +37,20 @@ namespace gr
     private:
       const double d_target_freq;
       const double d_samp_rate;
-      const double d_update_period;
+      const size_t d_update_period;
       const size_t d_est_thrhld;
+      const size_t d_corrections_per_sec;
 
+      gr::fxpt_nco d_nco;
+      doppler_fit d_doppler_fit_engine;
       double d_freq_diff;
       bool d_have_est;
       size_t d_freq_est_num;
-      std::deque<double> d_doppler_freqs;
+      size_t d_corrections;
+      size_t d_corrected_samples;
+      std::deque<freq_drift> d_doppler_freqs;
+      double *d_predicted_freqs;
+      gr_complex *d_nco_buff;
       boost::mutex d_mutex;
 
       void

lib/doppler_fit.cc

@@ -37,14 +37,19 @@ namespace gr
      * @param degree the degree of the polynomial
      */
     doppler_fit::doppler_fit (size_t degree) :
-	d_degree(degree)
-    {
-    }
-
-    doppler_fit::~doppler_fit ()
+	d_degree(degree),
+	d_last_x(0.0)
     {
     }
 
+    /**
+     * This method calculates the coefficients of the polynomial that will
+     * be used by the predict_freqs() method to produce simulated frequency
+     * differences
+     * @param drifts the queue containing the frequency differences and the
+     * corresponding timings that these frequencies diffrences occured. Time is
+     * measured in samples since the start of the flowgraph execution.
+     */
     void
     doppler_fit::fit (std::deque<freq_drift> drifts)
     {
@@ -85,8 +90,54 @@ namespace gr
       BOOST_ASSERT( singular == 0 );
 
       boost::numeric::ublas::lu_substitute(txx_matrix, perm, txy_matrix);
+
+      /*
+       * Lock the mutex to make sure that no one uses at the same time the
+       * coefficients
+       */
+      boost::mutex::scoped_lock lock(d_mutex);
       d_coeff = std::vector<double> (txy_matrix.data().begin(),
 				     txy_matrix.data().end());
+      d_last_x = drifts[s - 1].get_x();
+    }
+
+    /**
+     * Creates a number of frequency differences predictions using polynomial
+     * curve fitting.
+     * @param freqs buffer that will hold the predicted frequency differences.
+     * It is responsibility of the caller to provide enough memory for at most
+     * \p ncorrections double numbers.
+     * @param ncorrections the number predicted frequencies that the method
+     * will produce.
+     * @param samples_per_correction the number of samples elapsed between each
+     * correction.
+     */
+    void
+    doppler_fit::predict_freqs (double *freqs, size_t ncorrections,
+		     size_t samples_per_correction)
+    {
+      size_t i;
+      size_t j;
+      double predicted_freq_diff;
+      double x;
+      double xT;
+      boost::mutex::scoped_lock lock(d_mutex);
+      for(i = 0; i < ncorrections; i++){
+	predicted_freq_diff = 0.0;
+	xT = 1.0;
+	x = d_last_x + i * samples_per_correction;
+	for(j = 0; j < d_degree + 1; j++){
+	  predicted_freq_diff += d_coeff[j] * xT;
+	  xT *= x;
+	}
+	freqs[i] = predicted_freq_diff;
+      }
+
+      /*
+       * The predict method can be called multiple times without update the
+       * fitness of the polynomial. For this reason we alter the last x
+       */
+      d_last_x = d_last_x + (ncorrections + 1) * samples_per_correction;
     }
 
   } /* namespace satnogs */