/* -*- c++ -*- */ /* * Copyright 2018 hcab14@gmail.com. * * 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 #include #include #include "doppler_correction_cc_impl.h" namespace gr { namespace digitalhf { doppler_correction_cc::sptr doppler_correction_cc::make(unsigned int preamble_length, unsigned int preamble_length_cc) { return gnuradio::get_initial_sptr (new doppler_correction_cc_impl(preamble_length, preamble_length_cc)); } /* * The private constructor */ doppler_correction_cc_impl::doppler_correction_cc_impl(unsigned int preamble_length, unsigned int preamble_length_cc) : gr::sync_block("doppler_correction_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex))) , _preamble_length(preamble_length) , _preamble_length_cc(preamble_length_cc) , _rotator() , _state(WAIT_FOR_PHASE_EST_TAG) , _msg_metadata(pmt::make_dict()) , _port_name(pmt::intern("doppler")) , _phase_est(0.0f) , _amp_est_saved(1.0f) , _amp_est(1.0f) { GR_LOG_DECLARE_LOGPTR(d_logger); GR_LOG_ASSIGN_LOGPTR(d_logger, "doppler_correction_cc"); message_port_register_out(_port_name); message_port_register_in (_port_name); set_msg_handler(_port_name, boost::bind(&doppler_correction_cc_impl::handle_message, this, _1)); set_tag_propagation_policy(TPP_DONT); set_min_output_buffer(0, _preamble_length<<1); } doppler_correction_cc_impl::~doppler_correction_cc_impl() { } void doppler_correction_cc_impl::handle_message(pmt::pmt_t msg) { gr::thread::scoped_lock lock(d_setlock); bool const success = pmt::to_bool(pmt::dict_ref(msg, pmt::intern("success"), pmt::get_PMT_F())); if (!success) { // GR_LOG_DEBUG(d_logger, "next state > CONSUME_AND_SKIP success=false"); if (_state == WAIT_FOR_MSG) _state = CONSUME_AND_SKIP; return; } bool const use_amp_est = pmt::to_bool(pmt::dict_ref(msg, pmt::intern("use_amp_est"), pmt::get_PMT_T())); if (use_amp_est) _amp_est = _amp_est_saved; float const doppler = pmt::to_float(pmt::dict_ref(msg, pmt::intern("doppler"), pmt::from_float(0))); _rotator.set_phase_incr(gr_expj(-doppler)); if (_state == WAIT_FOR_MSG) { _rotator.set_phase(gr_expj(-_phase_est + 0.5*doppler*_preamble_length_cc)); _state = CONSUME_AND_INSERT_PREAMBLE_TAG; GR_LOG_DEBUG(d_logger, str(boost::format("next state > CONSUME_AND_INSERT_PREAMBLE_TAG phase_est=%f doppler=%f") % (_phase_est - 0.5*doppler*_preamble_length_cc) % doppler)); } } void doppler_correction_cc_impl::forecast(int noutput_items, gr_vector_int &ninput_items_required) { ninput_items_required[0] = std::max(int(_preamble_length + 1), noutput_items); // GR_LOG_DEBUG(d_logger, str(boost::format("forecast: %d %d %d") % noutput_items % ninput_items_required[0] % _preamble_length)); } int doppler_correction_cc_impl::work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr::thread::scoped_lock lock(d_setlock); gr_complex const* in = (gr_complex const*)input_items[0]; gr_complex *out = (gr_complex *) output_items[0]; if (noutput_items < _preamble_length) { GR_LOG_DEBUG(d_logger, str(boost::format("work: %d %d") % noutput_items % _preamble_length)); return 0; } noutput_items -= _preamble_length; int nout = 0; switch (_state) { case WAIT_FOR_PHASE_EST_TAG: { std::vector v_phase, v_amp; get_tags_in_window(v_phase, 0, 0, noutput_items, pmt::intern("phase_est")); get_tags_in_window(v_amp, 0, 0, noutput_items, pmt::intern("amp_est")); if (v_phase.empty()) { nout = noutput_items; } else { tag_t const& tag_phase = v_phase.front(); tag_t const& tag_amp = v_amp.front(); uint64_t const offset = nout = tag_phase.offset - nitems_read(0); _phase_est = pmt::to_double(tag_phase.value); _amp_est_saved = pmt::to_double(tag_amp.value); _msg_metadata = pmt::dict_add(_msg_metadata, pmt::intern("packet_len"), pmt::from_long(_preamble_length)); message_port_pub(_port_name, pmt::cons(_msg_metadata, pmt::init_c32vector(_preamble_length, in+nout))); // GR_LOG_DEBUG(d_logger, str(boost::format("next state > WAIT_FOR_MSG %lld phase_est=%f") % tag.offset % _phase_est)); _state = WAIT_FOR_MSG; } break; } // WAIT_FOR_PHASE_EST_TAG case WAIT_FOR_MSG: { // GR_LOG_DEBUG(d_logger, "WAIT_FOR_MSG"); // handle_message(delete_head_nowait(_port_name)); break; } // WAIT_FOR_MSG case CONSUME_AND_INSERT_PREAMBLE_TAG: { add_item_tag(0, nitems_read(0), pmt::intern("preamble_start"), pmt::from_long(0)); nout = _preamble_length; // GR_LOG_DEBUG(d_logger, str(boost::format("next state > WAIT_FOR_PHASE_EST_TAG %lld") % nitems_read(0))); _state = WAIT_FOR_PHASE_EST_TAG; break; } // CONSUME_AND_INSERT_PREAMBLE_TAG case CONSUME_AND_SKIP: { nout = _preamble_length; // GR_LOG_DEBUG(d_logger, str(boost::format("next state > WAIT_FOR_PHASE_EST_TAG %lld") % nitems_read(0))); _state = WAIT_FOR_PHASE_EST_TAG; break; } // CONSUME_AND_SKIP } // apply current doppler correction to all produced samples #ifdef USE_VOLK_ROTATOR _rotator.rotateN(out, in, nout); #else for (int i=0; i(out), reinterpret_cast(out), _amp_est, 2*nout); // Tell runtime system how many output items we produced. return nout; } bool doppler_correction_cc_impl::stop() { GR_LOG_DEBUG(d_logger, "doppler_correction stop"); return true; } } /* namespace digitalhf */ } /* namespace gr */