mirror of
https://github.com/f4exb/sdrangel.git
synced 2026-06-07 08:24:43 -04:00
Interferometer (2) compile
This commit is contained in:
f4exb
@@ -20,33 +20,21 @@
|
||||
#include "dsp/fftengine.h"
|
||||
#include "interferometercorr.h"
|
||||
|
||||
std::complex<float> fcAdd(std::complex<float>& a, const std::complex<float>& b) {
|
||||
return a + b;
|
||||
Sample sAdd(const Sample& a, const Sample& b) { //!< Sample addition
|
||||
return Sample{a.real() + b.real(), a.imag() + b.imag()};
|
||||
}
|
||||
|
||||
std::complex<float> fcMul(std::complex<float>& a, const std::complex<float>& b) {
|
||||
return a * b;
|
||||
Sample sMulConj(const Sample& a, const Sample& b) { //!< Sample multiply with conjugate
|
||||
return Sample{a.real()*b.real() + a.imag()*b.imag(), a.imag()*b.real() - a.real()*b.imag()};
|
||||
}
|
||||
|
||||
Sample cf2sAdd(std::complex<float>& a, const std::complex<float>& b)
|
||||
{
|
||||
std::complex<float> c = a + b;
|
||||
return Sample{c.real(), c.imag()};
|
||||
Sample cf2s(const std::complex<float>& a) { //!< Complex float to Sample
|
||||
Sample s;
|
||||
s.setReal(a.real()*SDR_RX_SCALEF);
|
||||
s.setImag(a.imag()*SDR_RX_SCALEF);
|
||||
return s;
|
||||
}
|
||||
|
||||
Sample cf2sMul(std::complex<float>& a, const std::complex<float>& b)
|
||||
{
|
||||
std::complex<float> c = a * b;
|
||||
return Sample{c.real(), c.imag()};
|
||||
}
|
||||
|
||||
Sample cf2s(std::complex<float>& a)
|
||||
{
|
||||
return Sample{a.real(), a.imag()};
|
||||
}
|
||||
|
||||
const unsigned int InterferometerCorrelator::m_nbFFTBlocks = 128;
|
||||
|
||||
InterferometerCorrelator::InterferometerCorrelator(int fftSize) :
|
||||
m_corrType(InterferometerSettings::CorrelationAdd),
|
||||
m_fftSize(fftSize)
|
||||
@@ -55,156 +43,158 @@ InterferometerCorrelator::InterferometerCorrelator(int fftSize) :
|
||||
{
|
||||
m_fft[i] = FFTEngine::create();
|
||||
m_fft[i]->configure(2*fftSize, false); // internally twice the data FFT size
|
||||
m_data[i] = new std::complex<float>[fftSize*m_nbFFTBlocks];
|
||||
m_dataIndex[i] = 0;
|
||||
}
|
||||
|
||||
m_invFFT = FFTEngine::create();
|
||||
m_invFFT->configure(2*fftSize, true);
|
||||
|
||||
m_dataj = new std::complex<float>[2*fftSize]; // receives actual FFT result hence twice the data FFT size
|
||||
m_scorr.resize(2*fftSize*m_nbFFTBlocks); // same size multiplied by the number of buffered FFT blocks
|
||||
m_tcorr.resize(2*fftSize*m_nbFFTBlocks); // same size multiplied by the number of buffered FFT blocks
|
||||
m_scorr.resize(2*fftSize);
|
||||
m_tcorr.resize(2*fftSize);
|
||||
}
|
||||
|
||||
InterferometerCorrelator::~InterferometerCorrelator()
|
||||
{
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
delete[] m_data[i];
|
||||
delete[] m_fft[i];
|
||||
}
|
||||
|
||||
delete[] m_dataj;
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int argIndex)
|
||||
void InterferometerCorrelator::performCorr(const SampleVector& data0, const SampleVector& data1)
|
||||
{
|
||||
if (argIndex > 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (m_corrType)
|
||||
{
|
||||
case InterferometerSettings::CorrelationAdd:
|
||||
feedOp(begin, end, argIndex, cf2sAdd);
|
||||
performOpCorr(data0, data1, sAdd);
|
||||
break;
|
||||
case InterferometerSettings::CorrelationMultiply:
|
||||
feedOp(begin, end, argIndex, cf2sMul);
|
||||
performOpCorr(data0, data1, sMulConj);
|
||||
break;
|
||||
case InterferometerSettings::CorrelationCorrelation:
|
||||
feedCorr(begin, end, argIndex);
|
||||
performFFTCorr(data0, data1);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::feedOp(
|
||||
const SampleVector::const_iterator& begin,
|
||||
const SampleVector::const_iterator& end,
|
||||
unsigned int argIndex,
|
||||
Sample complexOp(std::complex<float>& a, const std::complex<float>& b)
|
||||
)
|
||||
void InterferometerCorrelator::performOpCorr(const SampleVector& data0, const SampleVector& data1, Sample sampleOp(const Sample& a, const Sample& b))
|
||||
{
|
||||
int size = (end - begin);
|
||||
int fill = m_fftSize*m_nbFFTBlocks - m_dataIndex[argIndex];
|
||||
int first = std::min(fill, size);
|
||||
unsigned int size = std::min(data0.size(), data1.size());
|
||||
adjustTCorrSize(size);
|
||||
|
||||
std::transform(begin, begin + first, m_data[argIndex] + m_dataIndex[argIndex], [](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
});
|
||||
std::transform(
|
||||
data0.begin(),
|
||||
data0.begin() + size,
|
||||
data1.begin(),
|
||||
m_tcorr.begin(),
|
||||
sampleOp
|
||||
);
|
||||
|
||||
if (argIndex == 1)
|
||||
m_processed = size;
|
||||
m_remaining = 0;
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::performFFTCorr(const SampleVector& data0, const SampleVector& data1)
|
||||
{
|
||||
unsigned int size = std::min(data0.size(), data1.size());
|
||||
SampleVector::const_iterator begin0 = data0.begin();
|
||||
SampleVector::const_iterator begin1 = data1.begin();
|
||||
adjustSCorrSize(size);
|
||||
adjustTCorrSize(size);
|
||||
|
||||
while (size > m_fftSize)
|
||||
{
|
||||
// FFT[0]
|
||||
std::transform(
|
||||
m_data[0] + m_dataIndex[0], m_data[0] + m_dataIndex[0] + first, m_data[1] + m_dataIndex[1],
|
||||
m_tcorr.begin() + m_dataIndex[0],
|
||||
complexOp
|
||||
begin0,
|
||||
begin0 + m_fftSize,
|
||||
m_fft[0]->in(),
|
||||
[](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
}
|
||||
);
|
||||
std::fill(m_fft[0]->in() + m_fftSize, m_fft[0]->in() + 2*m_fftSize, std::complex<float>{0, 0});
|
||||
m_fft[0]->transform();
|
||||
|
||||
// FFT[1]
|
||||
std::transform(
|
||||
begin1,
|
||||
begin1 + m_fftSize,
|
||||
m_fft[1]->in(),
|
||||
[](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
}
|
||||
);
|
||||
std::fill(m_fft[1]->in() + m_fftSize, m_fft[1]->in() + 2*m_fftSize, std::complex<float>{0, 0});
|
||||
m_fft[1]->transform();
|
||||
|
||||
// conjugate FFT[1]
|
||||
std::transform(
|
||||
m_fft[1]->out(),
|
||||
m_fft[1]->out()+2*m_fftSize,
|
||||
m_dataj,
|
||||
[](const std::complex<float>& c) -> std::complex<float> {
|
||||
return std::conj(c);
|
||||
}
|
||||
);
|
||||
|
||||
emit dataReady(m_dataIndex[0], m_dataIndex[0] + first);
|
||||
// product of FFT[1]* with FFT[0] and store in inverse FFT input
|
||||
std::transform(
|
||||
m_fft[0]->out(),
|
||||
m_fft[0]->out()+2*m_fftSize,
|
||||
m_dataj,
|
||||
m_invFFT->in(),
|
||||
[](std::complex<float>& a, const std::complex<float>& b) -> std::complex<float> {
|
||||
return a*b;
|
||||
}
|
||||
);
|
||||
|
||||
// copy product to correlation spectrum
|
||||
std::transform(
|
||||
m_invFFT->in(),
|
||||
m_invFFT->in() + 2*m_fftSize,
|
||||
m_scorr.begin(),
|
||||
cf2s
|
||||
);
|
||||
|
||||
// do the inverse FFT to get time correlation
|
||||
m_invFFT->transform();
|
||||
std::transform(
|
||||
m_invFFT->out(),
|
||||
m_invFFT->out() + 2*m_fftSize,
|
||||
m_tcorr.begin(),
|
||||
cf2s
|
||||
);
|
||||
|
||||
// TODO: do something with the result
|
||||
size -= m_fftSize;
|
||||
begin0 += m_fftSize;
|
||||
begin1 += m_fftSize;
|
||||
}
|
||||
|
||||
if (size > fill)
|
||||
// update the samples counters
|
||||
m_processed = begin0 - data0.begin();
|
||||
m_remaining = size - m_fftSize;
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::adjustSCorrSize(int size)
|
||||
{
|
||||
if (size > m_scorrSize)
|
||||
{
|
||||
std::transform(begin, begin + size - fill , m_data[argIndex], [](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
});
|
||||
|
||||
if (argIndex == 1)
|
||||
{
|
||||
std::transform(
|
||||
m_data[0], m_data[0] + size - fill, m_data[1],
|
||||
m_tcorr.begin(),
|
||||
complexOp
|
||||
);
|
||||
|
||||
emit dataReady(0, size - fill);
|
||||
}
|
||||
|
||||
m_dataIndex[argIndex] = size - fill;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dataIndex[argIndex] += size;
|
||||
m_scorr.resize(size);
|
||||
m_scorrSize = size;
|
||||
}
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::feedCorr(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end, unsigned int argIndex)
|
||||
void InterferometerCorrelator::adjustTCorrSize(int size)
|
||||
{
|
||||
int size = (end - begin);
|
||||
int fill = m_fftSize*m_nbFFTBlocks - m_dataIndex[argIndex];
|
||||
int first = std::min(fill, size);
|
||||
|
||||
std::transform(begin, begin + first, m_data[argIndex] + m_dataIndex[argIndex], [](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
});
|
||||
processFFTBlocks(argIndex, m_dataIndex[argIndex], first);
|
||||
|
||||
if (size > fill)
|
||||
if (size > m_tcorrSize)
|
||||
{
|
||||
std::transform(begin, begin + size - fill, m_data[argIndex], [](const Sample& s) -> std::complex<float> {
|
||||
return std::complex<float>{s.real() / SDR_RX_SCALEF, s.imag() / SDR_RX_SCALEF};
|
||||
});
|
||||
processFFTBlocks(argIndex, 0, size - fill);
|
||||
m_dataIndex[argIndex] = size - fill;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dataIndex[argIndex] += size;
|
||||
m_tcorr.resize(size);
|
||||
m_tcorrSize = size;
|
||||
}
|
||||
}
|
||||
|
||||
void InterferometerCorrelator::processFFTBlocks(unsigned int argIndex, unsigned int dataIndex, int length)
|
||||
{
|
||||
int start = dataIndex / m_fftSize;
|
||||
int stop = (dataIndex + length) / m_fftSize;
|
||||
|
||||
for (int i = start; i < stop; i++)
|
||||
{
|
||||
m_window.apply(&m_data[argIndex][start*m_fftSize], m_fft[argIndex]->in());
|
||||
std::fill(m_fft[argIndex]->in() + m_fftSize, m_fft[argIndex]->in() + 2*m_fftSize, std::complex<float>{0,0});
|
||||
m_fft[argIndex]->transform();
|
||||
|
||||
if (argIndex == m_corrIndex)
|
||||
{
|
||||
// conjugate
|
||||
std::transform(m_fft[argIndex]->out(), m_fft[argIndex]->out()+2*m_fftSize, m_dataj, []
|
||||
(const std::complex<float>& c) -> std::complex<float> { return std::conj(c); }
|
||||
);
|
||||
// product with FFT[0] store in inverse FFT input
|
||||
std::transform(m_fft[0]->out(), m_fft[0]->out()+2*m_fftSize, m_dataj, m_invFFT->in(), []
|
||||
(std::complex<float>& a, const std::complex<float>& b) -> std::complex<float> { return a*b; }
|
||||
);
|
||||
// copy to correlation spectrum and do the inverse FFT to get time correlation
|
||||
std::transform(m_invFFT->in(), m_invFFT->in() + 2*m_fftSize, m_scorr.begin() + 2*i*m_fftSize, cf2s);
|
||||
m_invFFT->transform();
|
||||
std::transform(m_invFFT->out(), m_invFFT->out() + 2*m_fftSize, m_tcorr.begin() + 2*i*m_fftSize, cf2s);
|
||||
}
|
||||
}
|
||||
|
||||
if (start != stop) {
|
||||
emit dataReady(start, stop);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user