hamnet70-gnuradio-legacy/gr-hamnet70/lib/correct_frequency_from_pilot_syms_impl.cc

/* -*- c++ -*- */
/* 
 * Copyright 2019 Thomas Kolb.
 * 
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 * 
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <algorithm>

#include <gnuradio/io_signature.h>
#include <gnuradio/expj.h>
#include <gnuradio/math.h>
#include "correct_frequency_from_pilot_syms_impl.h"

namespace gr {
	namespace hamnet70 {

		correct_frequency_from_pilot_syms::sptr
		correct_frequency_from_pilot_syms::make(const std::vector<gr_complex> &pilot_sequence, const std::vector<size_t> &offsets, size_t phase_ref_offset, const std::string &start_tag)
		{
			return gnuradio::get_initial_sptr
				(new correct_frequency_from_pilot_syms_impl(pilot_sequence, offsets, phase_ref_offset, start_tag));
		}

		/*
		 * The private constructor
		 */
		correct_frequency_from_pilot_syms_impl::correct_frequency_from_pilot_syms_impl(const std::vector<gr_complex> &pilot_sequence, const std::vector<size_t> &offsets, size_t phase_ref_offset, const std::string &start_tag)
			: gr::block("correct_frequency_from_pilot_syms",
					gr::io_signature::make(1, 1, sizeof(gr_complex)),
					gr::io_signature::make(1, 1, sizeof(gr_complex))),
				d_pilot_sequence(pilot_sequence),
				d_offsets(offsets),
				d_phase_ref_offset(phase_ref_offset),
				d_start_tag(pmt::intern(start_tag)),
				d_items_required(*std::max_element(offsets.begin(), offsets.end()) + pilot_sequence.size()),
				d_tag_found(false),
				d_phase_inc_per_symbol(0.0f),
				d_current_phase(0.0f)
		{
			set_tag_propagation_policy(TPP_CUSTOM);
		}

		/*
		 * Our virtual destructor.
		 */
		correct_frequency_from_pilot_syms_impl::~correct_frequency_from_pilot_syms_impl()
		{
		}

		void
		correct_frequency_from_pilot_syms_impl::forecast (int noutput_items, gr_vector_int &ninput_items_required)
		{
			ninput_items_required[0] = noutput_items + d_pilot_sequence.size() * d_offsets.size();
		}

		int
		correct_frequency_from_pilot_syms_impl::general_work (int noutput_items,
				gr_vector_int &ninput_items,
				gr_vector_const_void_star &input_items,
				gr_vector_void_star &output_items)
		{
			const gr_complex *in = (const gr_complex *) input_items[0];
			gr_complex *out = (gr_complex *) output_items[0];

			int consumed = 0, produced = 0;

			std::vector<tag_t> tags;
			get_tags_in_window(tags, 0, 0, ninput_items[0], d_start_tag);
			size_t tagidx = 0;

			for(int i = 0; i < ninput_items[0]; i++) {
				if((tagidx < tags.size()) && (tags[tagidx].offset == nitems_read(0) + i)) {
					d_packet.clear();
					d_tags_in_packet.clear();
					d_tag_found = true;

					tagidx++;
				}

				if(!d_tag_found) {
					// tag propagation
					std::vector<tag_t> prop_tags;
					get_tags_in_window(prop_tags, 0, consumed, consumed+1);

					for(tag_t &tag: prop_tags) {
						tag.offset = nitems_written(0) + produced;
						add_item_tag(0, tag);
					}

					// calculate sample
					d_current_phase += d_phase_inc_per_symbol;
					out[produced++] = in[consumed++] * gr_expj(d_current_phase);
				} else if(d_tag_found && d_packet.size() < d_items_required) {
					// get and store tags for later insertion in the output stream
					std::vector<tag_t> prop_tags;
					get_tags_in_window(prop_tags, 0, consumed, consumed+1);

					for(tag_t &tag: prop_tags) {
						tag.offset = d_packet.size();
						d_tags_in_packet.push_back(tag);
					}

					d_packet.push_back(in[consumed++]);
				} else if(d_packet.size() == d_items_required) {

					// frequency estimation
					float phase_sum = 0.0f;
					size_t last_off = d_phase_ref_offset;
					float last_phase = 0.0f;

					// method 1: average over the complex vectors of the de-rotated pilot
					// symbols in each block, then calculate the phase and average the
					// phases change from one block to the next

					/*for(size_t off: d_offsets) {
						gr_complex sum(0, 0);

						size_t n = 0;
						for(const gr_complex &ref_sym: d_pilot_sequence) {
							sum += d_packet[off+n] * conj(ref_sym);
							n++;
						}

						sum /= n;

						float phase = gr::fast_atan2f(sum); // FIXME? unwrap
						phase_sum += (phase - last_phase) / (off - last_off + d_pilot_sequence.size()/2);
						last_phase = phase;
						last_off = off;
					}

					d_phase_inc_per_symbol = -phase_sum / d_offsets.size();*/

					// method 2: calculate the frequency offset for each pilot symbol in
					// each block with respect to the reference position. Then average
					// over all frequency offsets.

					float phase_inc_per_symbol_sum = 0.0f;

					for(size_t off: d_offsets) {

						size_t n = 0;
						for(const gr_complex &ref_sym: d_pilot_sequence) {
							gr_complex derot_symbol = d_packet[off+n] * conj(ref_sym);
							float phase = gr::fast_atan2f(derot_symbol);

							phase_inc_per_symbol_sum += phase / (off + n - d_phase_ref_offset);
							n++;
						}
					}

					d_phase_inc_per_symbol = -phase_inc_per_symbol_sum / (d_pilot_sequence.size() * d_offsets.size());

					d_current_phase = (-(int)d_phase_ref_offset - 1) * d_phase_inc_per_symbol;

					//std::cout << "initial phase: " << d_current_phase << std::endl;
					//std::cout << "phase increment: " << d_phase_inc_per_symbol << std::endl;

					size_t drop_start = 1;
					size_t drop_end = 0;
					size_t next_off_idx = 0;

					//std::cout << "packet = [";

					std::vector<tag_t>::iterator tag_iter = d_tags_in_packet.begin();

					for(size_t i = 0; i < d_packet.size(); i++) {
						d_current_phase += d_phase_inc_per_symbol;

						if(i == drop_end) {
							if(next_off_idx >= d_offsets.size()) {
								drop_end = drop_start = d_packet.size(); // index cannot be reached
							} else {
								drop_start = d_offsets[next_off_idx];
								drop_end = d_offsets[next_off_idx] + d_pilot_sequence.size();

								next_off_idx++;
							}
						}

						if(i >= drop_start) {
							continue;
						}

						// restore tags
						while(tag_iter != d_tags_in_packet.end() &&
								tag_iter->offset <= i) {
							tag_iter->offset = nitems_written(0) + produced;
							add_item_tag(0, *tag_iter);
							tag_iter++;
						}

						gr_complex &item = d_packet[i];
						out[produced++] = item * gr_expj(d_current_phase);

						gr_complex &c = out[produced-1];
						//std::cout << c.real() << "+" << c.imag() << "j, ";
					}

					//std::cout << "]" << std::endl;

					d_tag_found = false;
				}
			}


			// Tell runtime system how many input items we consumed on
			// each input stream.
			consume_each (consumed);

			// Tell runtime system how many output items we produced.
			return produced;
		}

	} /* namespace hamnet70 */
} /* namespace gr */