401 lines
12 KiB
C++
401 lines
12 KiB
C++
#include "gtl_math_fft.h"
|
|
|
|
namespace gtl {
|
|
namespace math {
|
|
|
|
fft::fft(gtl::analog_data *ad)
|
|
: QObject(ad)
|
|
, _ad(new gtl::math::filter_iir(ad, true))
|
|
, _rbw(10)//6.25
|
|
, _w(windows::rectangular)
|
|
, _nl(20)
|
|
, _ol(0)
|
|
, _done(false)
|
|
, _cn(4)
|
|
, _dr(1)
|
|
, _buffer_ptr(0)
|
|
, _buffer_ptr_offset(0)
|
|
{
|
|
_device = static_cast<gtl::device*>(_ad->root());
|
|
|
|
if(_device)
|
|
_device->lock_ai();
|
|
|
|
fftw_make_planner_thread_safe();
|
|
|
|
set_frequency(200);
|
|
init();
|
|
|
|
if(_device)
|
|
_device->unlock_ai();
|
|
|
|
connect(_ad, >l::analog_data::data_changed, this, &fft::data_changed);
|
|
}
|
|
|
|
|
|
fft::~fft()
|
|
{
|
|
emit deleting();
|
|
cleanup();
|
|
if(_ad)
|
|
delete _ad;
|
|
}
|
|
|
|
qreal fft::frequency() const
|
|
{
|
|
return _f;
|
|
}
|
|
|
|
void fft::set_frequency(qreal value)
|
|
{
|
|
if(_device)
|
|
_device->lock_ai();
|
|
|
|
if(value > _ad->get_rate()/2)
|
|
value = _ad->get_rate()/2;
|
|
|
|
if(_f != value && value <= _ad->get_rate()/2 && value > 0)
|
|
{
|
|
_f = value;
|
|
_dr = (uint) ( (_ad->get_rate()/2)/_f );
|
|
_fs = _ad->get_rate() / _dr;
|
|
_fk = _fs/2;
|
|
|
|
int size = _fs/_rbw;//_size_r = (size_t) _fs/_rbw;
|
|
_rbw = _fs / (qreal)size;//_rbw = _fs / (qreal)_size_r;
|
|
|
|
_nl = (uint) round(_f/_rbw);
|
|
|
|
init();
|
|
emit frequency_changed();
|
|
}
|
|
|
|
if(_device)
|
|
_device->unlock_ai();
|
|
}
|
|
|
|
qreal fft::resolution() const
|
|
{
|
|
return _rbw;
|
|
}
|
|
|
|
void fft::set_resolution(qreal value)
|
|
{
|
|
if(_device)
|
|
_device->lock_ai();
|
|
|
|
if(_rbw != value && _fk > value && value > 0)
|
|
{
|
|
_rbw = value;
|
|
|
|
int size = _fs/_rbw;
|
|
_rbw = _fs / (qreal)size;
|
|
|
|
_nl = (uint) round(_f/_rbw);
|
|
|
|
init();
|
|
emit resolution_changed();
|
|
}
|
|
|
|
if(_device)
|
|
_device->unlock_ai();
|
|
}
|
|
|
|
fft::windows fft::window() const
|
|
{
|
|
return _w;
|
|
}
|
|
|
|
void fft::set_window(windows value)
|
|
{
|
|
if(_w != value)
|
|
{
|
|
_w = value;
|
|
init();
|
|
emit window_changed();
|
|
}
|
|
}
|
|
|
|
int fft::lines() const
|
|
{
|
|
return _nl;
|
|
}
|
|
|
|
void fft::set_lines(int value)
|
|
{
|
|
if(_device)
|
|
_device->lock_ai();
|
|
|
|
if(_nl != value && value > 0)
|
|
{
|
|
// _nl = value;
|
|
// _rbw = _f/_nl;
|
|
set_resolution(_f/value);
|
|
// init();
|
|
emit lines_changed();
|
|
}
|
|
|
|
if(_device)
|
|
_device->unlock_ai();
|
|
}
|
|
|
|
qreal fft::overlap() const
|
|
{
|
|
return _ol;
|
|
}
|
|
|
|
void fft::set_overlap(int value)
|
|
{
|
|
if( value != _ol && value < 100 && value >= 0)
|
|
{
|
|
_ol = value;
|
|
init();
|
|
emit overlap_changed();
|
|
}
|
|
}
|
|
|
|
analog_data *fft::ad() const
|
|
{
|
|
return static_cast<analog_data*>(_ad->parent());
|
|
}
|
|
|
|
|
|
int fft::samples() const
|
|
{
|
|
return _samples;
|
|
}
|
|
|
|
int fft::size_fft_out() const
|
|
{
|
|
return _size_out;
|
|
}
|
|
|
|
int fft::size_fft_calc() const
|
|
{
|
|
return _size_r_k;
|
|
}
|
|
|
|
void fft::init()
|
|
{
|
|
_ad->lock_device();
|
|
cleanup();
|
|
|
|
_fk = _fs/2;
|
|
if(_fk < _rbw)
|
|
_rbw = _fk;
|
|
_size = _ad->get_rate()/_rbw;
|
|
_fs = _ad->get_rate()/_dr;
|
|
_size_r = ceil(_fs/_rbw);
|
|
_size_r_k = _dr > 1 ? _size_r/2 + 1 : _size_r/2;
|
|
_size_out = _nl < _size_r_k ? _nl + 1 : _nl;
|
|
_buffer_ptr = 0;
|
|
_buffer_ptr_offset = (int) (_size * _ol) / 100 ;
|
|
|
|
_buffer_in = new std::vector<qreal>(_size);
|
|
_buffer_w = new std::vector<qreal>(_size_r);
|
|
|
|
int N = _size_r;
|
|
switch (_w) {
|
|
case windows::rectangular:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [] () mutable { return 1.*1.051; });
|
|
break;
|
|
case windows::cosine:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = sin(M_PI*n/N); n++; return v*1.626; });
|
|
break;
|
|
case windows::hann:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.5 - 0.5*cos(2*M_PI*n/N); n++; return v*2.07; });
|
|
break;
|
|
case windows::bartlett_hann:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.62 - 0.48*fabs(n/N - 1/2) - 0.38*cos(2*M_PI*n/N); n++; return v*1.669; });
|
|
break;
|
|
case windows::hamming:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.54 - 0.46*cos(2*M_PI*n/N); n++; return v*1.918; });
|
|
break;
|
|
case windows::blackman:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.42 - 0.5*cos(2*M_PI*n/N) + 0.08*cos(4*M_PI*n/N); n++; return v*2.464; });
|
|
break;
|
|
case windows::blackman_harris:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.359 - 0.488*cos(2*M_PI*n/N) + 0.141*cos(4*M_PI*n/N) - 0.012*cos(6*M_PI*n/N); n++; return v*2.883; });
|
|
break;
|
|
case windows::flattop:
|
|
std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 1 - 1.93*cos(2*M_PI*n/N) + 1.29*cos(4*M_PI*n/N) - 0.388*cos(6*M_PI*n/N) + 0.032*cos(8*M_PI*n/N); n++; return v*1.035; });
|
|
break;
|
|
}
|
|
|
|
_fft_in = new std::vector<qreal>(_size_r);
|
|
|
|
resize(_size_r);
|
|
|
|
_plan = fftw_plan_dft_r2c_1d(_size_r, _fft_in->data(), reinterpret_cast<fftw_complex*>(data()), FFTW_ESTIMATE);
|
|
|
|
////ANTIALIAS FILTER
|
|
gtl::math::filter_iir *filter = static_cast<gtl::math::filter_iir*>(_ad);
|
|
filter->reset();
|
|
filter->set_kind(gtl::math::filter_iir::kinds::elliptic);
|
|
filter->set_type(gtl::math::filter_iir::types::lowpass);
|
|
filter->set_order(10);
|
|
filter->set_frequency(_rbw*_size_out);
|
|
filter->set_ripple(1);
|
|
filter->set_slope(1);
|
|
|
|
_ad->unlock_device();
|
|
|
|
_samples = 0;
|
|
|
|
emit initialized();
|
|
}
|
|
|
|
void fft::cleanup()
|
|
{
|
|
_ad->lock_device();
|
|
|
|
if(_plan)
|
|
{
|
|
fftw_destroy_plan(_plan);
|
|
_plan = nullptr;
|
|
}
|
|
|
|
if(_buffer_in)
|
|
{
|
|
_buffer_in->clear();
|
|
_buffer_in->shrink_to_fit();
|
|
delete _buffer_in;
|
|
_buffer_in = nullptr;
|
|
}
|
|
|
|
if(_buffer_w)
|
|
{
|
|
_buffer_w->clear();
|
|
_buffer_w->shrink_to_fit();
|
|
delete _buffer_w;
|
|
_buffer_w = nullptr;
|
|
}
|
|
|
|
if(_fft_in)
|
|
{
|
|
_fft_in->clear();
|
|
_fft_in->shrink_to_fit();
|
|
delete _fft_in;
|
|
_fft_in = nullptr;
|
|
}
|
|
|
|
clear();
|
|
shrink_to_fit();
|
|
|
|
_ad->unlock_device();
|
|
}
|
|
|
|
void fft::decimate(std::vector<qreal>::iterator in_it, std::vector<qreal>::iterator out_it, int size, int N, int R)
|
|
{
|
|
for( int i = 0; i < size/R; i++)
|
|
*(out_it + i) = *(in_it + i*R);
|
|
return;
|
|
}
|
|
|
|
void fft::set_rate(qreal value)
|
|
{
|
|
_ad->set_rate(value);
|
|
|
|
init();
|
|
}
|
|
|
|
void fft::data_changed()
|
|
{
|
|
_ad->lock_device();
|
|
|
|
_samples += _ad->size();
|
|
|
|
adcopy(_ad->size());
|
|
|
|
_ad->unlock_device();
|
|
}
|
|
|
|
void fft::adcopy(int adoffset)
|
|
{
|
|
if(_buffer_ptr < (int) _size)
|
|
{
|
|
if(adoffset > _size)
|
|
adcopy(adoffset - _size);
|
|
|
|
int adsize = _size;
|
|
|
|
if(adoffset >= _ad->size())
|
|
adsize = _ad->size() % _size;
|
|
|
|
std::vector<qreal>::iterator it_b = _ad->begin() + adoffset - (adoffset % _size);
|
|
std::vector<qreal>::iterator it_e = it_b + adsize;
|
|
|
|
/// Заполнение буфера данными
|
|
if( adsize >= _size)
|
|
{
|
|
if(_buffer_ptr)
|
|
{
|
|
int tmp = _buffer_ptr;
|
|
std::copy(it_b, it_e - _buffer_ptr, _buffer_in->begin() + _buffer_ptr);
|
|
_buffer_ptr = _size;
|
|
calc();
|
|
|
|
_buffer_ptr = tmp;
|
|
std::copy(it_e - _buffer_ptr, it_e, _buffer_in->begin());
|
|
}
|
|
else
|
|
{
|
|
std::copy(it_b, it_e, _buffer_in->begin());
|
|
_buffer_ptr = _size;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( (_size - _buffer_ptr) >= adsize )
|
|
{
|
|
std::copy(it_b, it_e, _buffer_in->begin() + _buffer_ptr);
|
|
_buffer_ptr += adsize;
|
|
}
|
|
else
|
|
{
|
|
int tmp = _buffer_ptr;
|
|
std::copy(it_b, it_b + (_size - _buffer_ptr), _buffer_in->begin() + _buffer_ptr);
|
|
_buffer_ptr = _size;
|
|
calc();
|
|
|
|
_buffer_ptr = std::distance(it_b, it_e) - (_size - tmp);
|
|
std::copy(it_e - _buffer_ptr, it_e, _buffer_in->begin());
|
|
}
|
|
}
|
|
/////////////////////
|
|
}
|
|
|
|
calc();
|
|
}
|
|
|
|
void fft::calc()
|
|
{
|
|
_done = false;
|
|
|
|
if( _buffer_ptr >= (int) _size && _plan )
|
|
{
|
|
////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
decimate(_buffer_in->begin(), _fft_in->begin(), _size, _cn, _dr);
|
|
|
|
// Весовая функция
|
|
for(int i = 0; i < _size_r; i++)
|
|
_fft_in->at(i) *= _buffer_w->at(i);
|
|
|
|
// БПФ
|
|
if(_plan) fftw_execute(_plan);
|
|
|
|
if( _buffer_ptr_offset )
|
|
std::copy(_buffer_in->end() - _buffer_ptr_offset, _buffer_in->end(), _buffer_in->begin());
|
|
_buffer_ptr = _buffer_ptr_offset;
|
|
|
|
_done = true;
|
|
}
|
|
|
|
if(_done)
|
|
emit changed();
|
|
}
|
|
|
|
} // namespace math
|
|
} // namespace gtl
|