From b96c84c401f36ea098a89d5f0a55e9e2da736630 Mon Sep 17 00:00:00 2001 From: Manolis Surligas Date: Wed, 30 Mar 2016 21:51:19 +0300 Subject: [PATCH] Continue working on the Doppler Correction block Actually the block seems to be ready, however proper testing is necessary --- include/satnogs/doppler_fit.h | 11 ++- lib/doppler_correction_cc_impl.cc | 113 ++++++++++++++++++++++++------ lib/doppler_correction_cc_impl.h | 14 +++- lib/doppler_fit.cc | 61 ++++++++++++++-- 4 files changed, 169 insertions(+), 30 deletions(-) diff --git a/include/satnogs/doppler_fit.h b/include/satnogs/doppler_fit.h index d290761..a9f9a3b 100644 --- a/include/satnogs/doppler_fit.h +++ b/include/satnogs/doppler_fit.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace gr { @@ -38,13 +39,19 @@ namespace gr { public: doppler_fit (size_t degree); - ~doppler_fit (); - void fit(std::deque drifts); + void + fit (std::deque 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 d_coeff; + boost::mutex d_mutex; }; } // namespace satnogs diff --git a/lib/doppler_correction_cc_impl.cc b/lib/doppler_correction_cc_impl.cc index 35c082e..d877dd5 100644 --- a/lib/doppler_correction_cc_impl.cc +++ b/lib/doppler_correction_cc_impl.cc @@ -24,6 +24,7 @@ #include #include "doppler_correction_cc_impl.h" +#include 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; diff --git a/lib/doppler_correction_cc_impl.h b/lib/doppler_correction_cc_impl.h index 646df73..4cca6dc 100644 --- a/lib/doppler_correction_cc_impl.h +++ b/lib/doppler_correction_cc_impl.h @@ -22,6 +22,9 @@ #define INCLUDED_SATNOGS_DOPPLER_CORRECTION_CC_IMPL_H #include +#include +#include +#include #include 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 d_doppler_freqs; + size_t d_corrections; + size_t d_corrected_samples; + std::deque d_doppler_freqs; + double *d_predicted_freqs; + gr_complex *d_nco_buff; boost::mutex d_mutex; void diff --git a/lib/doppler_fit.cc b/lib/doppler_fit.cc index e875ab2..76a3a94 100644 --- a/lib/doppler_fit.cc +++ b/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 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 (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 */