Continue working on the Doppler Correction block
Actually the block seems to be ready, however proper testing is necessary
This commit is contained in:
parent
5ac7fa4759
commit
b96c84c401
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 */
|
||||
|
|
Loading…
Reference in New Issue