134 lines
2.8 KiB
C++
134 lines
2.8 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2013-2021 Sylvain Munaut <tnt@246tNt.com>
|
|
*
|
|
* This file is part of gr-iqbalance
|
|
*
|
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <gnuradio/io_signature.h>
|
|
#include <gnuradio/iqbalance/optimize_c.h>
|
|
|
|
#define GCC_VERSION ( \
|
|
__GNUC__ * 10000 + \
|
|
__GNUC_MINOR__ * 100 + \
|
|
__GNUC_PATCHLEVEL__ \
|
|
)
|
|
|
|
#if GCC_VERSION >= 40800 || defined(__clang__)
|
|
# define complex _Complex
|
|
# undef _GLIBCXX_HAVE_COMPLEX_H
|
|
#endif
|
|
|
|
extern "C" {
|
|
#include <osmocom/dsp/cxvec.h>
|
|
#include <osmocom/dsp/iqbal.h>
|
|
}
|
|
|
|
#define FFT_SIZE 1024
|
|
#define FFT_COUNT 4
|
|
|
|
|
|
namespace gr {
|
|
|
|
iqbalance::optimize_c::sptr
|
|
iqbalance::optimize_c::make(int period)
|
|
{
|
|
return gnuradio::get_initial_sptr (new iqbalance::optimize_c(period));
|
|
}
|
|
|
|
iqbalance::optimize_c::optimize_c(int period)
|
|
: gr::sync_block ("optimize_c",
|
|
gr::io_signature::make(1, 1, sizeof (gr_complex)),
|
|
gr::io_signature::make(0, 0, 0)),
|
|
d_period(period), d_count(0), d_first(true), d_mag(0.0f), d_phase(0.0f)
|
|
{
|
|
message_port_register_out(pmt::mp("iqbal_corr"));
|
|
}
|
|
|
|
|
|
iqbalance::optimize_c::~optimize_c()
|
|
{
|
|
/* Nothing to do */
|
|
}
|
|
|
|
|
|
void
|
|
iqbalance::optimize_c::forecast(int noutput_items, gr_vector_int &ninput_items_required)
|
|
{
|
|
unsigned ninputs = ninput_items_required.size ();
|
|
for (unsigned i = 0; i < ninputs; i++)
|
|
ninput_items_required[i] = FFT_SIZE * FFT_COUNT;
|
|
}
|
|
|
|
|
|
int
|
|
iqbalance::optimize_c::work(int noutput_items,
|
|
gr_vector_const_void_star &input_items,
|
|
gr_vector_void_star &output_items)
|
|
{
|
|
const gr_complex *in = (const gr_complex *) input_items[0];
|
|
const int N = FFT_SIZE * FFT_COUNT;
|
|
struct osmo_cxvec _sig, *sig = &_sig;
|
|
struct osmo_iqbal_opts opts;
|
|
float p[2];
|
|
|
|
if (!this->d_period)
|
|
return noutput_items;
|
|
|
|
if (this->d_count) {
|
|
int missing = this->d_period - this->d_count;
|
|
|
|
if (missing <= 0) {
|
|
this->d_count = 0;
|
|
return 0;
|
|
} else if (missing >= noutput_items) {
|
|
this->d_count += noutput_items;
|
|
return noutput_items;
|
|
} else {
|
|
this->d_count = 0;
|
|
return missing;
|
|
}
|
|
}
|
|
|
|
if (noutput_items < N)
|
|
return 0;
|
|
|
|
this->d_count += N;
|
|
|
|
memcpy(&opts, &osmo_iqbal_default_opts, sizeof(opts));
|
|
opts.fft_size = FFT_SIZE;
|
|
opts.fft_count = FFT_COUNT;
|
|
opts.start_at_prev = 1;
|
|
|
|
p[0] = this->d_mag;
|
|
p[1] = this->d_phase;
|
|
|
|
osmo_cxvec_init_from_data(sig, (float complex *)in, N);
|
|
osmo_iqbal_cxvec_optimize(sig, &p[0], &p[1], &opts);
|
|
|
|
if (this->d_first) {
|
|
this->d_first = false;
|
|
this->d_mag = p[0];
|
|
this->d_phase = p[1];
|
|
} else {
|
|
this->d_mag = (0.95f * this->d_mag ) + (p[0] * 0.05f);
|
|
this->d_phase = (0.95f * this->d_phase) + (p[1] * 0.05f);
|
|
}
|
|
|
|
p[0] = this->d_mag;
|
|
p[1] = this->d_phase;
|
|
pmt::pmt_t msg = pmt::init_f32vector(2, p);
|
|
message_port_pub(pmt::mp("iqbal_corr"), msg);
|
|
|
|
return N;
|
|
}
|
|
|
|
} /* namespace gr */
|
|
|